diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index be95548ccb..871d91043e 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -8,7 +8,7 @@ on: jobs: build_and_push_docs: - runs-on: ubuntu-latest + runs-on: ubuntu-18.04 steps: - name: Checkout @@ -22,9 +22,10 @@ jobs: ref: gh-pages - name: Install dependencies run: | - sudo apt-get install python-bson - sudo apt-get install flex - sudo apt-get install graphviz + sudo apt-get install -y bison + sudo apt-get install -y flex + sudo apt-get install -y graphviz + sudo apt-get install -y python-setuptools - name: Install doxygen run: | wget 'https://sourceforge.net/projects/doxygen/files/rel-1.8.20/doxygen-1.8.20.src.tar.gz' diff --git a/CMakeLists.txt b/CMakeLists.txt index 479fbdb383..605c3394b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(Astrobee) -set(ASTROBEE_VERSION 0.14.1) +set(ASTROBEE_VERSION 0.14.3) # Define our options option(USE_CCACHE @@ -209,9 +209,17 @@ find_package(Gflags REQUIRED) ADD_DEFINITIONS(-DFREEFLYER_GFLAGS_NAMESPACE=${GFLAGS_NAMESPACE}) enable_testing() +if (USE_CTC) + find_program(LSB_RELEASE_EXEC lsb_release) + execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --codename OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + if("${LSB_RELEASE_VERSION_SHORT}" STREQUAL "bionic") + set(Protobuf_PROTOC_EXECUTABLE /usr/local/bin/protoc) + endif() +endif (USE_CTC) + find_package(Protobuf REQUIRED) if (NOT PROTOBUF_PROTOC_EXECUTABLE) - message(FATAL_ERROR "Could not find system's protoc execuable") + message(FATAL_ERROR "Could not find system's protoc executable") endif (NOT PROTOBUF_PROTOC_EXECUTABLE) find_package(Ceres REQUIRED) @@ -239,6 +247,11 @@ if (${OpenCV_VERSION} MATCHES "3.3.1") endif() endforeach(__cvcomponent) endif() +if (USE_CTC) + foreach(__cvcomponent ${OpenCV_LIB_COMPONENTS}) + set(OpenCV_LIBS ${OpenCV_LIBS} "${OpenCV_INSTALL_PATH}/lib/arm-linux-gnueabihf/lib${__cvcomponent}3.so") + endforeach(__cvcomponent) +endif (USE_CTC) set(OpenCV_LIBRARIES ${OpenCV_LIBS}) find_package(dbow2 REQUIRED) @@ -308,7 +321,13 @@ if (USE_CTC) endif (USE_CTC) if (USE_ROS) set(ENV{ROS_LANG_DISABLE} "genlisp:gennodejs:geneus:$ENV{ROS_LANG_DISABLE}") - find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet rviz) + + if (USE_CTC) + find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet) + else () + find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet rviz) + endif (USE_CTC) + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) if (IS_BAMBOO_BUILD) diff --git a/RELEASE.md b/RELEASE.md index be7678d663..33d595ce71 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,12 @@ # Releases +## Release 0.14.3 + + * Rotation fallback fix + * Unified install instructions and Ubuntu 18 nasa install + * Various other minor fixes + * Note, Perching does not work in this release, use another version for this. + ## Release 0.14.1 * Dynamic IMU Filtering diff --git a/astrobee.doxyfile b/astrobee.doxyfile index 114b3f615a..15399fef38 100644 --- a/astrobee.doxyfile +++ b/astrobee.doxyfile @@ -39,7 +39,7 @@ PROJECT_NAME = "NASA Astrobee Robot Software" # control system is used. -PROJECT_NUMBER = 0.14.1 +PROJECT_NUMBER = 0.14.3 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a @@ -912,7 +912,8 @@ EXAMPLE_RECURSIVE = NO # \image command). IMAGE_PATH = doc/images \ - doc/gen/png + doc/gen/png \ + localization/sparse_mapping # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program diff --git a/astrobee/commands/freeFlyerPlanSchema.json b/astrobee/commands/freeFlyerPlanSchema.json index 5097536bd0..ac53e1149c 100644 --- a/astrobee/commands/freeFlyerPlanSchema.json +++ b/astrobee/commands/freeFlyerPlanSchema.json @@ -50,17 +50,6 @@ ] }, - { - "type": "ParamSpec", - "id": "Data.DownloadMethod", - "valueType": "string", - "notes": "Specifies which log to operate on. Typically, the \"Immediate\" log is for high-priority telemetry that should be downloaded immediately after the activity, and the \"Delayed\" log is for other telemetry.", - "choices": [ - ["Immediate", "Immediate"], - ["Delayed", "Delayed"] - ] - }, - { "type": "ParamSpec", "id": "Settings.FlightMode", @@ -132,7 +121,7 @@ "type": "ParamSpec", "id": "Settings.PlannerType", "valueType": "string", - "notes": "Which planner to use:", + "notes": "Which planner to use:", "choices": [ ["trapezoidal", "Trapezoidal"], ["qp", "QuadraticProgram"] @@ -192,7 +181,7 @@ "type": "CommandSpec", "id": "Mobility.simpleMove6DOF", "isRapidNative": true, - "notes": "Moves the robot to the specified pose.

Using the default trapezoidal planner, the move has the following steps:
- If not in holonomic mode, the robot rotates to face the target position.
- Once rotation is complete, the robot translates in a straight line to the target position.
- Once translation is complete, the robot rotates to the target attitude.", + "notes": "Moves the robot to the specified pose.

Using the default trapezoidal planner, the move works as follows:
- If holonomic mode is disabled (usual case), the robot will sequentially (1) rotate to face the target position, (2) translate to the target position while facing forward, (3) rotate to the target attitude.
- If holonomic mode is enabled, the robot will simultaneously translate along a straight line to the target position while rotating to the target attitude.", "availableContexts": [ "teleop" ], @@ -356,19 +345,6 @@ "params": [] }, - { - "type": "CommandSpec", - "id": "Admin.wipeHlp", - "isRapidNative": false, - "notes": "Not implemented. This command is intended to erase certain data on the Astrobee HLP's local storage.", - "javaNotes": "Do not use this command. It will erase your running APK.", - "availableContexts": [ - "teleop" - ], - "parent": "Command", - "params": [] - }, - { "type": "CommandSpec", "id": "Mobility.idlePropulsion", @@ -382,66 +358,6 @@ "params": [] }, - { - "type": "CommandSpec", - "id": "Data.downloadData", - "isRapidNative": false, - "oldFswAlias": "startDownload", - "notes": "Not implemented. This command is intended to start downloading certain data logs stored onboard the robot.

Currently, downlink is initiated manually via SSH and rsync, managed by a terminal menu configured in the astrobee_ops repo. See also IRG-FFTEST217 Astrobee Wrap-Up and Shutdown.", - "javaNotes": "Data can only be downloaded when docked.", - "availableContexts": [ - "teleop", - "planner" - ], - "parent": "Command", - "params": [ - { - "type": "ParamSpec", - "id": "dataMethod", - "parent": "Data.DownloadMethod" - } - ] - }, - - { - "type": "CommandSpec", - "id": "Data.stopDownload", - "isRapidNative": false, - "isStopCommand": true, - "stopCommandType": "Data.downloadData", - "notes": "Not implemented. This command is intended to stop downloading data. See Data.downloadData.", - "availableContexts": [ - "teleop" - ], - "parent": "Command", - "params": [ - { - "type": "ParamSpec", - "id": "dataMethod", - "parent": "Data.DownloadMethod" - } - ] - }, - - { - "type": "CommandSpec", - "id": "Data.clearData", - "isRapidNative": false, - "notes": "Not implemented. This command is intended to clear certain data logs onboard the robot after they have been downloaded, in order to free storage space.

Currently, onboard storage is managed as follows:

  1. Prior to an activity, review what files have been downloaded after previous activities, and specify which files are ready be deleted from onboard storage in the Test Readiness Review notes on the FFFSW Confluence wiki.
  2. Manually delete those files (via SSH session) at the start of the commanding window, before executing the activity.

See also IRG-FFTEST207a Astrobee Quick Wakeup and Checkout.", - "availableContexts": [ - "teleop", - "planner" - ], - "parent": "Command", - "params": [ - { - "type": "ParamSpec", - "id": "dataMethod", - "parent": "Data.DownloadMethod" - } - ] - }, - { "type": "CommandSpec", "id": "Mobility.dock", @@ -772,7 +688,7 @@ "type": "ParamSpec", "id": "targetAngularVelocity", "valueType": "float", - "unit": "deg/s", + "unit": "rad/s", "minimum": 0, "notes": "The maximum angular velocity to target while rotating" }, @@ -780,7 +696,7 @@ "type": "ParamSpec", "id": "targetAngularAcceleration", "valueType": "float", - "unit": "deg/s^2", + "unit": "rad/s^2", "minimum": 0, "notes": "The maximum angular acceleration to target while rotating" }, @@ -799,7 +715,7 @@ "type": "CommandSpec", "id": "Settings.setHolonomicMode", "isRapidNative": false, - "notes": "Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam forward in order to enable obstacle detection. When in holonomic mode, the default trapezoidal planner will skip its usual first step of slewing attitude to face toward the target position. Instead, it will generate a trajectory that maintains the robot's previous attitude during translation to the target position.", + "notes": "Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam in the direction of motion while translating in order to enable obstacle detection. When holonomic mode is enabled, the default trapezoidal planner will simultaneously translate to the target position and rotate to the target attitude.", "availableContexts": [ "teleop", "planner" @@ -986,18 +902,6 @@ ] }, - { - "type": "CommandSpec", - "id": "Admin.shutdown", - "isRapidNative": true, - "notes": "Not implemented. This command is intended to put Astrobee in hibernate power mode.

An Astrobee that is hibernating can't receive Astrobee API commands. However, it can later be remotely awakened, if it is on the dock and not physically switched off. See the procedure IRG-FFTEST217 Astrobee Wrap-Up and Shutdown for current instructions on how to shut down an Astrobee, using a terminal session.", - "availableContexts": [ - "teleop" - ], - "parent": "Command", - "params": [] - }, - { "type": "CommandSpec", "id": "Mobility.stopAllMotion", @@ -1132,32 +1036,6 @@ "params": [] }, - { - "type": "CommandSpec", - "id": "Settings.genericCommand", - "isRapidNative": false, - "notes": "Invokes a 'generic' command, i.e., one that was added to the Astrobee API after the Astrobee control station code freeze. At present, no such commands are defined.", - "availableContexts": [ - "teleop", - "planner" - ], - "parent": "Command", - "params": [ - { - "type": "ParamSpec", - "id": "commandName", - "valueType": "string", - "notes": "Which generic command to invoke." - }, - { - "type": "ParamSpec", - "id": "param", - "valueType": "string", - "notes": "The parameters for the generic command, usually formatted as a JSON dictionary. This allows any number of parameters, with arbitrary types." - } - ] - }, - { "type": "CommandSpec", "id": "Admin.loadNodelet", @@ -1240,7 +1118,7 @@ "type": "CommandSpec", "id": "Settings.setEnableImmediate", "isRapidNative": false, - "notes": "For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. There could be up to a half a second of delay between the robots starting their plans. See also Settings.setTimeSync. Astrobee to Astrobee communication is also in the works, and may eventually enable synchronizing Astrobees in a better way.

Note: Immediate mode motion control is not related to immediate download (as in Data.setDataToDisk).", + "notes": "For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. Astrobee to Astrobee communication is in the works, and may eventually enable synchronizing Astrobees in a better way.", "availableContext": [ "teleop" ], @@ -1257,28 +1135,28 @@ { "type": "CommandSpec", - "id": "Settings.setTimeSync", + "id": "Settings.setPlanner", "isRapidNative": false, - "notes": "For expert use only. Enables 'discrete time unit' synchronization for multiple Astrobees.

Enabling time sync changes how Astrobee initiates motion along each trajectory segment. It delays the start of motion until the time is an integer multiple of the discrete time unit, a configurable value that is currently set to five seconds. The idea is that if an operator tries to start two robots moving at the same time, even if there is some differential delay in how long the command takes to arrive at each robot, as long as both robots receive the command within the same discrete time unit window, they will start at the same moment (i.e., the beginning of the next window).

However, we can not recommend enabling time sync to achieve synchronization at this time, due to the following concerns: (1) execution with time sync has never really been tested, and may be buggy, (2) the synchronization protocol is not robust: one can not guarantee that the different robots will receive the command within the same discrete time unit window.", + "notes": "Switches which trajectory planner is used when Astrobee needs to generate a trajectory to reach a target pose. Astrobee uses its trajectory planner when it receives a teleoperation motion command. However, when an fplan is generated by the Astrobee control station plan editor, it normally contains pre-computed trajectories for its motion segments; when Astrobee executes these fplans, the onboard trajectory planner is not used.", "availableContext": [ - "teleop" + "teleop", + "planner" ], "parent": "Command", "params": [ { "type": "ParamSpec", - "id": "setTimeSync", - "valueType": "boolean", - "notes": "Set to true/false to enable/disable discrete time unit synchronization." + "id": "planner", + "parent": "Settings.PlannerType" } ] }, { "type": "CommandSpec", - "id": "Settings.setPlanner", + "id": "Settings.setEnableReplan", "isRapidNative": false, - "notes": "Switches which trajectory planner is used when Astrobee needs to generate a trajectory to reach a target pose. Astrobee uses its trajectory planner when it receives a teleoperation motion command. However, when an fplan is generated by the Astrobee control station plan editor, it normally contains pre-computed trajectories for its motion segments; when Astrobee executes these fplans, the onboard trajectory planner is not used.", + "notes": "Allows Astrobee to re-plan if it detects an obstacle too close to its forward trajectory. Enabling replanning only makes sense when you are using a planner that is able to plan around obstacles. (See Setting.setPlanner. As of 5/2021, only the QP planner can plan around obstacles).", "availableContext": [ "teleop", "planner" @@ -1287,8 +1165,9 @@ "params": [ { "type": "ParamSpec", - "id": "planner", - "parent": "Settings.PlannerType" + "id": "enableReplan", + "valueType": "boolean", + "notes": "If true, when Astrobee detects an obstacle too close to its forward trajectory, after the robot comes to a stop, the choreographer will automatically request a new trajectory from the current configured planner. If false, the robot will stop and wait for operator assistance." } ] }, diff --git a/astrobee/config/commands.config b/astrobee/config/commands.config index 06b8302128..aa69b5bba6 100644 --- a/astrobee/config/commands.config +++ b/astrobee/config/commands.config @@ -66,10 +66,6 @@ commandConfig = { name="resetEkf", parameters={} }, - { - name="shutdown", - parameters={} - }, { name="switchLocalization", parameters={ @@ -113,10 +109,6 @@ commandConfig = { type="RAPID_INT" } } - }, - { - name="wipeHlp", - parameters={} } }, name="AdminType" @@ -162,24 +154,6 @@ commandConfig = { }, { commands={ - { - name="clearData", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, - { - name="downloadData", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, { name="setDataToDisk", parameters={} @@ -193,15 +167,6 @@ commandConfig = { } } }, - { - name="stopDownload", - parameters={ - { - key="dataMethod", - type="RAPID_STRING" - } - } - }, { name="stopRecording", parameters={} @@ -363,19 +328,6 @@ commandConfig = { }, { commands={ - { - name="genericCommand", - parameters={ - { - key="commandName", - type="RAPID_STRING" - }, - { - key="param", - type="RAPID_STRING" - } - } - }, { name="setCamera", parameters={ @@ -463,6 +415,15 @@ commandConfig = { } } }, + { + name="setEnableReplan", + parameters={ + { + key="enableReplan", + type="RAPID_BOOL" + } + } + }, { name="setFlashlightBrightness", parameters={ @@ -561,15 +522,6 @@ commandConfig = { } } }, - { - name="setTimeSync", - parameters={ - { - key="setTimeSync", - type="RAPID_BOOL" - } - } - }, { name="setZones", parameters={} diff --git a/astrobee/config/graph_localizer.config b/astrobee/config/graph_localizer.config index b955234454..ad6f5d842f 100644 --- a/astrobee/config/graph_localizer.config +++ b/astrobee/config/graph_localizer.config @@ -17,7 +17,7 @@ require "context" -- Graph Value Options -ideal_duration = 3.5 +ideal_duration = 3.25 -- Don't leave less than min_num_states in window if possible min_num_states = 3 max_num_states = 20 @@ -42,7 +42,6 @@ huber_k = world_huber_k ignore_gravity = true estimate_world_T_dock_using_loc = true -- Applies to factor adders but not standstill detection -optical_flow_measurement_spacing = 2 -- Sanity Checker options -- TODO(rsoussan): Create seperate config for sanity checker? check_pose_difference = true @@ -58,15 +57,19 @@ starting_prior_quaternion_stddev = 0.01 starting_prior_velocity_stddev = 0.01 starting_prior_accel_bias_stddev = 0.001 starting_prior_gyro_bias_stddev = 0.001 +-- Threshold Bias Uncertainies +threshold_bias_uncertainty = true +accel_bias_stddev_threshold = 0.025 +gyro_bias_stddev_threshold = 0.025 -- Lost Threshold position_cov_log_det_lost_threshold = 0 orientation_cov_log_det_lost_threshold = 0 -- Feature Tracker -feature_tracker_sliding_window_duration = 3.5 +feature_tracker_sliding_window_duration = 3.25 -- Standstill max_standstill_feature_track_avg_distance_from_mean = 0.075 standstill_min_num_points_per_track = 4 -standstill_feature_tracker_sliding_window_duration = 1 +standstill_feature_track_duration = 1 standstill_adder_add_velocity_prior = true standstill_adder_add_pose_between_factor = true standstill_adder_prior_velocity_stddev = 0.01 @@ -82,13 +85,16 @@ smart_projection_adder_dynamic_outlier_rejection_threshold = 50 smart_projection_adder_retriangulation_threshold = 1e-5 smart_projection_adder_verbose_cheirality = false smart_projection_adder_robust = true -smart_projection_adder_max_num_factors = 10 +smart_projection_adder_max_num_factors = 13 smart_projection_adder_min_num_points = 2 -smart_projection_adder_max_num_points_per_factor = 5 -smart_projection_adder_rotation_only_fallback = false +smart_projection_adder_max_num_points_per_factor = 7 +smart_projection_adder_measurement_spacing = 2 +smart_projection_adder_feature_track_min_separation = 0 +smart_projection_adder_rotation_only_fallback = true smart_projection_adder_splitting = true smart_projection_adder_scale_noise_with_num_points = true smart_projection_adder_noise_scale = 2 +smart_projection_adder_use_allowed_timestamps = true -- Optical Flow Projection Factors projection_adder_enabled = false projection_adder_enable_EPI = false @@ -101,8 +107,8 @@ projection_adder_point_prior_translation_stddev = 0.1 -- Loc Factors loc_adder_add_pose_priors = false loc_adder_add_projections = true -loc_adder_prior_translation_stddev = 0.01 -loc_adder_prior_quaternion_stddev = 0.01 +loc_adder_prior_translation_stddev = 0.06 +loc_adder_prior_quaternion_stddev = 0.06 loc_adder_min_num_matches = 5 loc_adder_max_num_factors = 50 loc_adder_scale_pose_noise_with_num_landmarks = true diff --git a/astrobee/config/hw/pmc_actuator.config b/astrobee/config/hw/pmc_actuator.config index dd64e97042..482b2fef27 100644 --- a/astrobee/config/hw/pmc_actuator.config +++ b/astrobee/config/hw/pmc_actuator.config @@ -42,6 +42,9 @@ i2c_retries = 3 -- Control rate in Hz control_rate_hz = 62.5 +-- Maximum PMC time without control input +max_timeout = 2.0 + -- The null speed and nozzle positions null_speed = 0 null_nozzle_positions = {128, 128, 128, 128, 128, 128} diff --git a/astrobee/config/management/data_bagger.config b/astrobee/config/management/data_bagger.config index 1f3bed440e..f68b3e38c0 100644 --- a/astrobee/config/management/data_bagger.config +++ b/astrobee/config/management/data_bagger.config @@ -51,6 +51,7 @@ default_topics = {{topic="gnc/ctl/traj", downlink="immediate", frequency=-1}, {topic="command", downlink="immediate", frequency=-1}, {topic="mgt/ack", downlink="immediate", frequency=-1}, {topic="mgt/sys_monitor/time_diff", downlink="immediate", frequency=-1}, + {topic="rosout", downlink="immediate", frequency=-1}, {topic="robot_name", downlink="immediate", frequency=-1}} require "context" diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index 7f6784240b..10d235a4e3 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -25,16 +25,15 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { --- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.08017845188659993, 0.0015130070850712233, -0.0037397072731650606), quat4(-0.010795173083773568, -0.020686270289023867, 0.99972637753705862, -0.0016469718428887884)), - scicam_to_hazcam_transform = transform(vec3(0.039812511450892832, 0.017762378646844504, -0.0039477526753304197), quat4(-0.0017211102261497006, -0.023078433050076921, 0.99972939958575946, -0.0023561221189618764)), + hazcam_to_navcam_transform = transform(vec3(0.070656217, -0.0053120391, -0.0086391367), quat4(-0.0033938631, 0.011939978, 0.99990009, -0.0067619677)), + scicam_to_hazcam_transform = transform(vec3(0.043949838, -0.026925687, -0.14459291), quat4(-0.009399956, 0.015955961, 0.99982772, -0.0012581665)), navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.31, - hazcam_depth_to_image_transform = { - 0.9672, 0.0017, 0.0111, -0.0060, - -0.0016, 0.9672, -0.0065, 0.0040, - -0.0112, 0.0065, 0.9672, -0.0133, - 0.0, 0.0, 0.0, 1.0}, + scicam_to_hazcam_timestamp_offset = 0.5, + hazcam_depth_to_image_transform = { + 0.91122714, -0.00020224138, -0.00066071236, 0.00091477566, + 0.00020680677, 0.91120557, 0.0063030044, -0.0072143461, + 0.00065929762, -0.0063031525, 0.91120536, -0.0035329091, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! @@ -77,10 +76,10 @@ robot_camera_calibrations = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.411106, 0.375288, -0.00080993, -0.00817328}, + distortion_coeff = {-0.050689743, -1.1461691, -0.001373226, -0.00056427513}, intrinsic_matrix = { - 188.27394, 0.0, 117.94881, - 0.0, 186.18359, 87.740547, + 215.88697, 0.0, 114.12908, + 0.0, 215.88697, 86.547331, 0.0, 0.0, 1.0 }, gain=50, @@ -88,10 +87,10 @@ robot_camera_calibrations = { }, -- Placeholder for sci_cam, not accurate! sci_cam = { - distortion_coeff = {0.128628, -0.167456, 0.00213421, -0.00262211}, + distortion_coeff = {0.10733913, -0.10935864, 0.0010663099, 0.0010407278}, intrinsic_matrix = { - 859.44015, 0.0, 754.24485, - 0.0, 849.35466, 487.7349, + 1138.4943, 0.0, 680.36447, + 0.0, 1138.4943, 534.00133, 0.0, 0.0, 1.0 }, gain=50, diff --git a/astrobee/config/tools/graph_bag.config b/astrobee/config/tools/graph_bag.config index cac1ad6826..55969624f5 100644 --- a/astrobee/config/tools/graph_bag.config +++ b/astrobee/config/tools/graph_bag.config @@ -33,4 +33,5 @@ ar_min_msg_spacing = 0 -- graph_localizer_simulator optimization_time = 0.30 -- Other -save_optical_flow_images = false; +save_optical_flow_images = false +log_relative_time = false diff --git a/astrobee/config/worlds/granite.config b/astrobee/config/worlds/granite.config index 00baead1cc..65895a872c 100755 --- a/astrobee/config/worlds/granite.config +++ b/astrobee/config/worlds/granite.config @@ -151,9 +151,9 @@ world_flight_modes = { speed = 2; -- Tolerances - tolerance_pos = 0.15; -- Position (10 cm) + tolerance_pos = 0.2; -- Position (20 cm) tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) + tolerance_att = 0.3491; -- Attitude (20 degrees) tolerance_omega = 0; -- Omega (disabled) tolerance_time = 1.0; -- Time (for sync check) diff --git a/astrobee/config/worlds/iss.config b/astrobee/config/worlds/iss.config index 7f43b0c3c1..73c70dae7a 100755 --- a/astrobee/config/worlds/iss.config +++ b/astrobee/config/worlds/iss.config @@ -160,9 +160,9 @@ world_flight_modes = { speed = 2; -- Tolerances - tolerance_pos = 0.1; -- Position (10 cm) + tolerance_pos = 0.4; -- Position (40 cm) tolerance_vel = 0; -- Velocity (disabled) - tolerance_att = 0.1745; -- Attitude (10 degrees) + tolerance_att = 0.3491; -- Attitude (20 degrees) tolerance_omega = 0; -- Omega (disabled) tolerance_time = 1.0; -- Time (for sync check) diff --git a/astrobee/launch/astrobee.launch b/astrobee/launch/astrobee.launch index e00b14a02b..41c5ca138f 100644 --- a/astrobee/launch/astrobee.launch +++ b/astrobee/launch/astrobee.launch @@ -134,9 +134,9 @@ name="ROS_HOSTNAME" value="$(arg mlp)" /> - - diff --git a/astrobee/launch/sim.launch b/astrobee/launch/sim.launch index 2ad5ebf0a2..aaeb06ae2c 100644 --- a/astrobee/launch/sim.launch +++ b/astrobee/launch/sim.launch @@ -50,7 +50,7 @@ - + @@ -142,7 +142,7 @@ - + @@ -221,4 +221,7 @@ + + + diff --git a/astrobee/scripts/cpu_print_version.sh b/astrobee/scripts/cpu_print_version.sh index 53dd8a1158..190127cd6c 100755 --- a/astrobee/scripts/cpu_print_version.sh +++ b/astrobee/scripts/cpu_print_version.sh @@ -15,12 +15,14 @@ echo "Kernel: $(uname -r)" lsb_release -r echo "Ros $(rosversion -d) $(rosversion roscpp)" echo "Debians:" -echo " astrobee0: $(deb_version astrobee0)" +echo " astrobee-comms: $(deb_version astrobee-comms)" +echo " astrobee0: $(deb_version astrobee0)" echo " astrobee-config: $(deb_version astrobee-config)" echo " astrobee-avionics: $(deb_version astrobee-avionics)" echo "FSW Version:" cat /opt/astrobee/version.txt | sed 's/^/ /' echo "Modified Files:" +dpkg -V astrobee-comms | sed 's/^/ /' dpkg -V astrobee0 | sed 's/^/ /' dpkg -V astrobee-config | sed 's/^/ /' dpkg -V astrobee-avionics | sed 's/^/ /' diff --git a/communications/dds_msgs/idl/AstrobeeCommandConstants.idl b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl index 912b3d88fa..4a65e11e47 100644 --- a/communications/dds_msgs/idl/AstrobeeCommandConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl @@ -96,9 +96,6 @@ module rapid { const rapid::String32 ADMIN_METHOD_WAKE_SAFE_PARAM_BERTH_NUMBER = "berthNumber"; const rapid::DataType ADMIN_METHOD_WAKE_SAFE_DTYPE_BERTH_NUMBER = rapid::RAPID_INT; - //@copy-declaration /** Not implemented. This command is intended to erase certain data on the Astrobee HLP's local storage. */ - const rapid::String32 ADMIN_METHOD_WIPE_HLP = "wipeHlp"; - const rapid::String32 ARM = "Arm"; //@copy-declaration /** Moves Astrobee's arm.

The arm has two joints. The tilt joint is used to deploy/stow the arm and adjust the SciCam tilt angle while perched. The pitch joint is used to adjust the SciCam pan angle while perched.

The (pan, tilt) = (0, 0) reference position is defined to have the arm fully deployed and aligned with the robot's -X axis. If perched on a handrail on an ISS wall, this position should nominally make the SciCam camera axis point directly toward the opposite wall. Increasing the tilt angle tilts the SciCam up, and increasing the pan angle pans the SciCam to the right. The arm's stowed position is (pan, tilt) = (0, 180).

The arm joints will be moved sequentially:

  1. Pan: If which is "Pan" or "Both", pan to the specified pan angle.
  2. Tilt: If which is "Tilt" or "Both", tilt to the specified tilt angle.

Naturally, if you prefer to tilt first, you can issue a tilt-only move, followed by a pan-only move.

Some mistakes to avoid include:

*/ @@ -131,20 +128,6 @@ module rapid { const rapid::String32 DATA = "Data"; - //@copy-declaration /** Not implemented. This command is intended to clear certain data logs onboard the robot after they have been downloaded, in order to free storage space.

Currently, onboard storage is managed as follows:

  1. Prior to an activity, review what files have been downloaded after previous activities, and specify which files are ready be deleted from onboard storage in the Test Readiness Review notes on the FFFSW Confluence wiki.
  2. Manually delete those files (via SSH session) at the start of the commanding window, before executing the activity.

See also IRG-FFTEST207a Astrobee Quick Wakeup and Checkout. */ - const rapid::String32 DATA_METHOD_CLEAR_DATA = "clearData"; - - //@copy-declaration /** Specifies which log to operate on. Typically, the "Immediate" log is for high-priority telemetry that should be downloaded immediately after the activity, and the "Delayed" log is for other telemetry. */ - const rapid::String32 DATA_METHOD_CLEAR_DATA_PARAM_DATA_METHOD = "dataMethod"; - const rapid::DataType DATA_METHOD_CLEAR_DATA_DTYPE_DATA_METHOD = rapid::RAPID_STRING; - - //@copy-declaration /** Not implemented. This command is intended to start downloading certain data logs stored onboard the robot.

Currently, downlink is initiated manually via SSH and rsync, managed by a terminal menu configured in the astrobee_ops repo. See also IRG-FFTEST217 Astrobee Wrap-Up and Shutdown. */ - const rapid::String32 DATA_METHOD_DOWNLOAD_DATA = "downloadData"; - - //@copy-declaration /** Specifies which log to operate on. Typically, the "Immediate" log is for high-priority telemetry that should be downloaded immediately after the activity, and the "Delayed" log is for other telemetry. */ - const rapid::String32 DATA_METHOD_DOWNLOAD_DATA_PARAM_DATA_METHOD = "dataMethod"; - const rapid::DataType DATA_METHOD_DOWNLOAD_DATA_DTYPE_DATA_METHOD = rapid::RAPID_STRING; - //@copy-declaration /** Sets the data-to-disk configuration, which specifies how to log ROS telemetry topics to the robot's onboard storage.

The configuration is specified in a JSON-formatted file that contains a list of topic entries. For each logged topic, one specifies:

Sample data-to-disk configuration files can be found in astrobee_ops/gds/ControlStationConfig.

The Astrobee control station implements the protocol for managing onboard telemetry recording:

  1. Uplink a new data-to-disk file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Data.setDataToDisk command to load the uplinked file. Any errors in the file will be reported at this time.
  3. Use the Data.startRecording / Data.stopRecording commands to start / stop onboard telemetry logging.
*/ const rapid::String32 DATA_METHOD_SET_DATA_TO_DISK = "setDataToDisk"; @@ -155,13 +138,6 @@ module rapid { const rapid::String32 DATA_METHOD_START_RECORDING_PARAM_DESCRIPTION = "description"; const rapid::DataType DATA_METHOD_START_RECORDING_DTYPE_DESCRIPTION = rapid::RAPID_STRING; - //@copy-declaration /** Not implemented. This command is intended to stop downloading data. See Data.downloadData. */ - const rapid::String32 DATA_METHOD_STOP_DOWNLOAD = "stopDownload"; - - //@copy-declaration /** Specifies which log to operate on. Typically, the "Immediate" log is for high-priority telemetry that should be downloaded immediately after the activity, and the "Delayed" log is for other telemetry. */ - const rapid::String32 DATA_METHOD_STOP_DOWNLOAD_PARAM_DATA_METHOD = "dataMethod"; - const rapid::DataType DATA_METHOD_STOP_DOWNLOAD_DTYPE_DATA_METHOD = rapid::RAPID_STRING; - //@copy-declaration /** Stops logging ROS telemetry to onboard storage, as initiated by Data.startRecording. See also Data.setDataToDisk. */ const rapid::String32 DATA_METHOD_STOP_RECORDING = "stopRecording"; @@ -258,17 +234,6 @@ module rapid { const rapid::String32 SETTINGS = "Settings"; - //@copy-declaration /** Invokes a 'generic' command, i.e., one that was added to the Astrobee API after the Astrobee control station code freeze. At present, no such commands are defined. */ - const rapid::String32 SETTINGS_METHOD_GENERIC_COMMAND = "genericCommand"; - - //@copy-declaration /** Which generic command to invoke. */ - const rapid::String32 SETTINGS_METHOD_GENERIC_COMMAND_PARAM_COMMAND_NAME = "commandName"; - const rapid::DataType SETTINGS_METHOD_GENERIC_COMMAND_DTYPE_COMMAND_NAME = rapid::RAPID_STRING; - - //@copy-declaration /** The parameters for the generic command, usually formatted as a JSON dictionary. This allows any number of parameters, with arbitrary types. */ - const rapid::String32 SETTINGS_METHOD_GENERIC_COMMAND_PARAM_PARAM = "param"; - const rapid::DataType SETTINGS_METHOD_GENERIC_COMMAND_DTYPE_PARAM = rapid::RAPID_STRING; - //@copy-declaration /** Sets camera parameters.

The Astrobee camera control life cycle is as follows:

*/ const rapid::String32 SETTINGS_METHOD_SET_CAMERA = "setCamera"; @@ -335,13 +300,20 @@ module rapid { const rapid::String32 SETTINGS_METHOD_SET_ENABLE_AUTO_RETURN_PARAM_ENABLE_AUTO_RETURN = "enableAutoReturn"; const rapid::DataType SETTINGS_METHOD_SET_ENABLE_AUTO_RETURN_DTYPE_ENABLE_AUTO_RETURN = rapid::RAPID_BOOL; - //@copy-declaration /** For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. There could be up to a half a second of delay between the robots starting their plans. See also Settings.setTimeSync. Astrobee to Astrobee communication is also in the works, and may eventually enable synchronizing Astrobees in a better way.

Note: Immediate mode motion control is not related to immediate download (as in Data.setDataToDisk). */ + //@copy-declaration /** For expert use only. Changes the semantics of how Astrobee executes fplan trajectory segments.

A segment specifies the motion trajectory between stations in the plan. Within the segment, desired pose and velocity are smoothly interpolated functions of time over a time interval [t0, t1]. Plans created by the Astrobee control station plan editor always specify each segment's time values relative to the start of that segment's execution (i.e. t0 = 0). These plans must be executed in immediate mode, so called because when execution reaches a new segment, the executive and choreographer immediately 'start the clock' on executing the timed trajectory. In principle, if you want to synchronize motion of multiple robots, it could be useful to have the interval [t0, t1] specified using absolute timestamps. That is the behavior when immediate mode is disabled. The timestamp t0 is interpreted as absolute time using ROS conventions (usually UNIX epoch when running on real hardware, but could be any arbitrary time scale when running in simulation), and the start of motion on the segment would be delayed until the current time t = t0.

At this time, we cannot recommend disabling immediate mode to achieve synchronization, due to the following concerns: (1) execution with absolute timestamps has never really been tested, and may be buggy, (2) the control station plan editor doesn't provide any way to generate segments with absolute timestamps, (3) since it is seldom possible to predict exactly when ISS conditions will be right to begin a multi-robot activity, it would be awkward in practice to have the exact absolute timing of segments hard-coded into the plans.

As an alternative way to synchronize motion, you can execute with immediate mode enabled as usual, but take special care to minimize any skew in start time. Upload the plans in advance, use the Mobility.prepare command to get the robots ready to move, and then run the plans simultaneously. Astrobee to Astrobee communication is in the works, and may eventually enable synchronizing Astrobees in a better way. */ const rapid::String32 SETTINGS_METHOD_SET_ENABLE_IMMEDIATE = "setEnableImmediate"; //@copy-declaration /** Set to true/false to enable/disable immediate mode motion control. */ const rapid::String32 SETTINGS_METHOD_SET_ENABLE_IMMEDIATE_PARAM_ENABLE_IMMEDIATE = "enableImmediate"; const rapid::DataType SETTINGS_METHOD_SET_ENABLE_IMMEDIATE_DTYPE_ENABLE_IMMEDIATE = rapid::RAPID_BOOL; + //@copy-declaration /** Allows Astrobee to re-plan if it detects an obstacle too close to its forward trajectory. Enabling replanning only makes sense when you are using a planner that is able to plan around obstacles. (See Setting.setPlanner. As of 5/2021, only the QP planner can plan around obstacles). */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_REPLAN = "setEnableReplan"; + + //@copy-declaration /** If true, when Astrobee detects an obstacle too close to its forward trajectory, after the robot comes to a stop, the choreographer will automatically request a new trajectory from the current configured planner. If false, the robot will stop and wait for operator assistance. */ + const rapid::String32 SETTINGS_METHOD_SET_ENABLE_REPLAN_PARAM_ENABLE_REPLAN = "enableReplan"; + const rapid::DataType SETTINGS_METHOD_SET_ENABLE_REPLAN_DTYPE_ENABLE_REPLAN = rapid::RAPID_BOOL; + //@copy-declaration /** Sets flashlight brightness. */ const rapid::String32 SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS = "setFlashlightBrightness"; @@ -353,7 +325,7 @@ module rapid { const rapid::String32 SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_PARAM_BRIGHTNESS = "brightness"; const rapid::DataType SETTINGS_METHOD_SET_FLASHLIGHT_BRIGHTNESS_DTYPE_BRIGHTNESS = rapid::RAPID_FLOAT; - //@copy-declaration /** Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam forward in order to enable obstacle detection. When in holonomic mode, the default trapezoidal planner will skip its usual first step of slewing attitude to face toward the target position. Instead, it will generate a trajectory that maintains the robot's previous attitude during translation to the target position. */ + //@copy-declaration /** Enables/disables holonomic mode.

Holonomic mode is sometimes called 'blind flying' because it relaxes the constraint to always point the HazCam in the direction of motion while translating in order to enable obstacle detection. When holonomic mode is enabled, the default trapezoidal planner will simultaneously translate to the target position and rotate to the target attitude. */ const rapid::String32 SETTINGS_METHOD_SET_HOLONOMIC_MODE = "setHolonomicMode"; //@copy-declaration /** Set to true/false to enable/disable holonomic mode. */ @@ -413,7 +385,7 @@ module rapid { //@copy-declaration /** Switches which trajectory planner is used when Astrobee needs to generate a trajectory to reach a target pose. Astrobee uses its trajectory planner when it receives a teleoperation motion command. However, when an fplan is generated by the Astrobee control station plan editor, it normally contains pre-computed trajectories for its motion segments; when Astrobee executes these fplans, the onboard trajectory planner is not used. */ const rapid::String32 SETTINGS_METHOD_SET_PLANNER = "setPlanner"; - //@copy-declaration /** Which planner to use:

*/ + //@copy-declaration /** Which planner to use: */ const rapid::String32 SETTINGS_METHOD_SET_PLANNER_PARAM_PLANNER = "planner"; const rapid::DataType SETTINGS_METHOD_SET_PLANNER_DTYPE_PLANNER = rapid::RAPID_STRING; @@ -428,13 +400,6 @@ module rapid { const rapid::String32 SETTINGS_METHOD_SET_TELEMETRY_RATE_PARAM_RATE = "rate"; const rapid::DataType SETTINGS_METHOD_SET_TELEMETRY_RATE_DTYPE_RATE = rapid::RAPID_FLOAT; - //@copy-declaration /** For expert use only. Enables 'discrete time unit' synchronization for multiple Astrobees.

Enabling time sync changes how Astrobee initiates motion along each trajectory segment. It delays the start of motion until the time is an integer multiple of the discrete time unit, a configurable value that is currently set to five seconds. The idea is that if an operator tries to start two robots moving at the same time, even if there is some differential delay in how long the command takes to arrive at each robot, as long as both robots receive the command within the same discrete time unit window, they will start at the same moment (i.e., the beginning of the next window).

However, we can not recommend enabling time sync to achieve synchronization at this time, due to the following concerns: (1) execution with time sync has never really been tested, and may be buggy, (2) the synchronization protocol is not robust: one can not guarantee that the different robots will receive the command within the same discrete time unit window. */ - const rapid::String32 SETTINGS_METHOD_SET_TIME_SYNC = "setTimeSync"; - - //@copy-declaration /** Set to true/false to enable/disable discrete time unit synchronization. */ - const rapid::String32 SETTINGS_METHOD_SET_TIME_SYNC_PARAM_SET_TIME_SYNC = "setTimeSync"; - const rapid::DataType SETTINGS_METHOD_SET_TIME_SYNC_DTYPE_SET_TIME_SYNC = rapid::RAPID_BOOL; - //@copy-declaration /** Loads the most recently uplinked keepout zones file.

The Astrobee control station implements the protocol for managing keepout zones:

  1. Uplink a new keepout zone file using the RAPID compressed file protocol over DDS. (File uplink is not considered a command, so it does not appear in this command dictionary).
  2. Send this Settings.setZones command to load the uplinked zones file. Any errors in the file will be reported at this time.
  3. Use the Settings.setCheckZones command to enable/disable keepout zone checking.
*/ const rapid::String32 SETTINGS_METHOD_SET_ZONES = "setZones"; @@ -449,9 +414,6 @@ module rapid { const rapid::String32 ARM_ACTION_TYPE_TILT = "Tilt"; const rapid::String32 ARM_ACTION_TYPE_BOTH = "Both"; - const rapid::String32 DATA_DOWNLOAD_METHOD_IMMEDIATE = "Immediate"; - const rapid::String32 DATA_DOWNLOAD_METHOD_DELAYED = "Delayed"; - const rapid::String32 POWER_POWERED_COMPONENT_LASER_POINTER = "Laser Pointer"; const rapid::String32 POWER_POWERED_COMPONENT_PAYLOAD_TOP_AFT = "Payload Top Aft"; const rapid::String32 POWER_POWERED_COMPONENT_PAYLOAD_BOTTOM_AFT = "Payload Bottom Aft"; diff --git a/communications/dds_msgs/idl/MobilitySettingsState.idl b/communications/dds_msgs/idl/MobilitySettingsState.idl index 7efcc0f4de..985e2f44d6 100644 --- a/communications/dds_msgs/idl/MobilitySettingsState.idl +++ b/communications/dds_msgs/idl/MobilitySettingsState.idl @@ -60,16 +60,17 @@ module rapid { //@copy-declaration * a LOS. */ public boolean enableAutoReturn; - //@copy-declaration /** Set this to true to allow mobility to possibly - //@copy-declaration * help sync moves between multiple Astrobees. */ - public boolean timeSyncEnabled; - //@copy-declaration /** Set this to false if mobility should start the //@copy-declaration * a segment based on its time stamp. */ public boolean immediateEnabled; //@copy-declaration /** Sets the planner mobility uses. */ public string planner; + + //@copy-declaration /** Setting this to true allows Astrobee to re-plan + //@copy-declaration * if there is an obstacle to close to the robot's + //@copy-declaration * forward trajectory. */ + public boolean replanningEnabled; }; }; }; diff --git a/communications/dds_ros_bridge/src/ros_agent_state.cc b/communications/dds_ros_bridge/src/ros_agent_state.cc index ea19a338b9..170810c518 100644 --- a/communications/dds_ros_bridge/src/ros_agent_state.cc +++ b/communications/dds_ros_bridge/src/ros_agent_state.cc @@ -172,8 +172,8 @@ void ff::RosAgentStateToRapid::Callback( mob_msg.checkObstacles = status->check_obstacles; mob_msg.checkKeepouts = status->check_zones; mob_msg.enableAutoReturn = status->auto_return_enabled; - mob_msg.timeSyncEnabled = status->time_sync_enabled; mob_msg.immediateEnabled = status->immediate_enabled; + mob_msg.replanningEnabled = status->replanning_enabled; // Currently the code only supports planners 32 characters long if (status->planner.size() > 32) { diff --git a/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc b/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc index 22496e7199..079e385d49 100644 --- a/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc +++ b/communications/dds_ros_bridge/src/ros_compressed_image_rapid_image.cc @@ -63,8 +63,12 @@ std::string RosCompressedImageRapidImage::GetRapidMimeType( // only two accepted values jpeg or png if (ros_format.compare("jpeg") == 0) return rapid::MIME_IMAGE_JPEG; + if (ros_format.compare("mono8; jpeg compressed ") == 0) + return rapid::MIME_IMAGE_JPEG; if (ros_format.compare("png") == 0) return rapid::MIME_IMAGE_PNG; + + ROS_ERROR_STREAM("DDS ROS Bridge: Unknown camera format: " << ros_format << "."); return ""; } diff --git a/communications/ff_msgs/msg/AgentStateStamped.msg b/communications/ff_msgs/msg/AgentStateStamped.msg index 1d9fa16173..43e84455d6 100644 --- a/communications/ff_msgs/msg/AgentStateStamped.msg +++ b/communications/ff_msgs/msg/AgentStateStamped.msg @@ -76,10 +76,6 @@ bool check_zones # it will auto return without checking this flag bool auto_return_enabled -# Specifies whether timesync is enabled. Hopefully this will be used to -# synchronize movements between Astrobees. -bool time_sync_enabled - # Specifies whether the choreographer should execute a segment immediately or # based on the time stamp in the segement bool immediate_enabled @@ -87,6 +83,9 @@ bool immediate_enabled # Specifies the current planner being used string planner +# Specifies whether re-planning is allowed +bool replanning_enabled + # Specifies the current world being used string world diff --git a/communications/ff_msgs/msg/CommandConstants.msg b/communications/ff_msgs/msg/CommandConstants.msg index 5e05585372..f84a319a60 100644 --- a/communications/ff_msgs/msg/CommandConstants.msg +++ b/communications/ff_msgs/msg/CommandConstants.msg @@ -11,8 +11,6 @@ string PARAM_NAME_LOCALIZATION_MODE_TRUTH = Truth string PARAM_NAME_ACTION_TYPE_PAN = Pan string PARAM_NAME_ACTION_TYPE_TILT = Tilt string PARAM_NAME_ACTION_TYPE_BOTH = Both -string PARAM_NAME_DOWNLOAD_METHOD_IMMEDIATE = Immediate -string PARAM_NAME_DOWNLOAD_METHOD_DELAYED = Delayed string PARAM_NAME_POWERED_COMPONENT_LASER_POINTER = Laser Pointer string PARAM_NAME_POWERED_COMPONENT_PAYLOAD_TOP_AFT = Payload Top Aft string PARAM_NAME_POWERED_COMPONENT_PAYLOAD_BOTTOM_AFT = Payload Bottom Aft @@ -61,22 +59,17 @@ string CMD_NAME_LOAD_NODELET = loadNodelet string CMD_NAME_NO_OP = noOp string CMD_NAME_REACQUIRE_POSITION = reacquirePosition string CMD_NAME_RESET_EKF = resetEkf -string CMD_NAME_SHUTDOWN = shutdown string CMD_NAME_SWITCH_LOCALIZATION = switchLocalization string CMD_NAME_UNLOAD_NODELET = unloadNodelet string CMD_NAME_UNTERMINATE = unterminate string CMD_NAME_WAKE = wake string CMD_NAME_WAKE_SAFE = wakeSafe -string CMD_NAME_WIPE_HLP = wipeHlp string CMD_NAME_ARM_PAN_AND_TILT = armPanAndTilt string CMD_NAME_GRIPPER_CONTROL = gripperControl string CMD_NAME_STOP_ARM = stopArm string CMD_NAME_STOW_ARM = stowArm -string CMD_NAME_CLEAR_DATA = clearData -string CMD_NAME_DOWNLOAD_DATA = downloadData string CMD_NAME_SET_DATA_TO_DISK = setDataToDisk string CMD_NAME_START_RECORDING = startRecording -string CMD_NAME_STOP_DOWNLOAD = stopDownload string CMD_NAME_STOP_RECORDING = stopRecording string CMD_NAME_CUSTOM_GUEST_SCIENCE = customGuestScience string CMD_NAME_START_GUEST_SCIENCE = startGuestScience @@ -97,7 +90,6 @@ string CMD_NAME_SKIP_PLAN_STEP = skipPlanStep string CMD_NAME_WAIT = wait string CMD_NAME_POWER_OFF_ITEM = powerOffItem string CMD_NAME_POWER_ON_ITEM = powerOnItem -string CMD_NAME_GENERIC_COMMAND = genericCommand string CMD_NAME_SET_CAMERA = setCamera string CMD_NAME_SET_CAMERA_RECORDING = setCameraRecording string CMD_NAME_SET_CAMERA_STREAMING = setCameraStreaming @@ -105,13 +97,13 @@ string CMD_NAME_SET_CHECK_OBSTACLES = setCheckObstacles string CMD_NAME_SET_CHECK_ZONES = setCheckZones string CMD_NAME_SET_ENABLE_AUTO_RETURN = setEnableAutoReturn string CMD_NAME_SET_ENABLE_IMMEDIATE = setEnableImmediate +string CMD_NAME_SET_ENABLE_REPLAN = setEnableReplan string CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS = setFlashlightBrightness string CMD_NAME_SET_HOLONOMIC_MODE = setHolonomicMode string CMD_NAME_SET_INERTIA = setInertia string CMD_NAME_SET_OPERATING_LIMITS = setOperatingLimits string CMD_NAME_SET_PLANNER = setPlanner string CMD_NAME_SET_TELEMETRY_RATE = setTelemetryRate -string CMD_NAME_SET_TIME_SYNC = setTimeSync string CMD_NAME_SET_ZONES = setZones string CMD_SUBSYS_ACCESS_CONTROL = AccessControl diff --git a/debian/.gitignore b/debian/.gitignore index 0297c35c51..dbddbc6754 100644 --- a/debian/.gitignore +++ b/debian/.gitignore @@ -1,5 +1,6 @@ tmp astrobee0 +astrobee-comms astrobee-dev astrobee-config files diff --git a/debian/astrobee-comms.dirs b/debian/astrobee-comms.dirs new file mode 100644 index 0000000000..cdd16ade47 --- /dev/null +++ b/debian/astrobee-comms.dirs @@ -0,0 +1,4 @@ +opt/astrobee +opt/astrobee/lib +opt/astrobee/lib/pkgconfig +opt/astrobee/share diff --git a/debian/astrobee-comms.files b/debian/astrobee-comms.files new file mode 100644 index 0000000000..074fa03445 --- /dev/null +++ b/debian/astrobee-comms.files @@ -0,0 +1,3 @@ +opt/astrobee/lib/libdds_ros_bridge.so +opt/astrobee/lib/librapidExtAstrobee.so +opt/astrobee/share/dds_ros_bridge diff --git a/debian/changelog b/debian/changelog index 23894fbaa1..f8a55d4ccb 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,12 @@ +astrobee (0.14.3) testing; urgency=medium + + * Rotation fallback fix + * Unified install instructions and Ubuntu 18 nasa install + * Various other minor fixes + * Note, Perching does not work in this release, use another version for this. + + -- Astrobee Flight Software Mon, 24 May 2021 23:17:32 -0700 + astrobee (0.14.1) testing; urgency=medium * Dynamic IMU Filtering diff --git a/debian/control b/debian/control index 5053260b57..9b314c5631 100644 --- a/debian/control +++ b/debian/control @@ -28,10 +28,26 @@ Depends: ${misc:Depends} Description: Astrobee flight software configuration. The Astrobee flight software configuration files. +Package: astrobee-comms +Section: comm +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} + astrobee-config (>= ${binary:Version}), + rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0), + ros-kinetic-ros-base, ros-kinetic-cpp-common, + ros-kinetic-common-msgs, ros-kinetic-nodelet, ros-kinetic-image-transport, + ros-kinetic-compressed-image-transport, ros-kinetic-tf2, + ros-kinetic-tf2-geometry-msgs, ros-kinetic-tf2-msgs, ros-kinetic-tf2-ros, + ros-kinetic-tf2-sensor-msgs, + libgoogle-glog0v5 +Description: The communications for Astrobee flight software. + The communications portion of the Astrobee flight software. + Package: astrobee0 Architecture: any Depends: ${shlibs:Depends}, ${misc:Depends} astrobee-config (>= ${binary:Version}), + astrobee-comms (>= ${binary:Version}), libalvar2 (>=2.0), libdbow21 (>=0.1), libgtsam (>=4.0), libopenmvg1 (>=1.0), libroyale1 (>=1.0), libceres1 (>=1.0), rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0), libdecomputil0 (>=0.1), libjps3d0 (>=0.1), diff --git a/debian/rules b/debian/rules index 67dde28591..9ac1033ac5 100755 --- a/debian/rules +++ b/debian/rules @@ -23,6 +23,8 @@ ifeq ($(DEB_HOST_ARCH),armhf) export LIB_SEARCH_OPTIONS = -l$(ARMHF_CHROOT_DIR)/opt/rti/ndds/lib/armv6vfphLinux3.xgcc4.7.2:$(ARMHF_CHROOT_DIR)/opt/ros/kinetic/lib:$(ARMHF_CHROOT_DIR)/opt/ros/kinetic/lib/arm-linux-gnueabihf:$(ARMHF_CHROOT_DIR)/lib:$(ARMHF_CHROOT_DIR)/usr/lib:$(ARMHF_CHROOT_DIR)/usr/lib/arm-linux-gnueabihf:$(ARMHF_CHROOT_DIR)/lib/arm-linux-gnueabihf -- --ignore-missing-info --admindir=$(ARMHF_CHROOT_DIR)/var/lib/dpkg export DEB_MAINT_CC_SET = $(ARMHF_TOOLCHAIN)/bin/arm-linux-gnueabihf-gcc export DEB_MAINT_CXX_SET = $(ARMHF_TOOLCHAIN)/bin/arm-linux-gnueabihf-g++ +else + export LIB_SEARCH_OPTIONS = -l/opt/ros/kinetic/lib:/opt/ros/kinetic/lib/x86_64-linux-gnu:/usr/lib/x86_64-linux-gnu/gazebo-7/plugins endif # skip build for config files @@ -37,6 +39,10 @@ override_dh_auto_configure: mkdir -p build cd build && cmake .. -DCMAKE_VERBOSE_MAKEFILE=ON -DCMAKE_INSTALL_PREFIX:PATH=/opt/astrobee -DCMAKE_BUILD_TYPE=Release $(EXTRA_CMAKE_OPTS) && cd .. +override_dh_install: + dh_movefiles + dh_install + override_dh_auto_test: ; #override_dh_auto_install-idep: diff --git a/description/description/readme.md b/description/description/readme.md index f129657bc1..6de9890f9b 100644 --- a/description/description/readme.md +++ b/description/description/readme.md @@ -5,16 +5,16 @@ The Universal Robot Description Format (URDF) is a language for describing a rob This folder contains only the Xacro files for the free-flyer model. The dock, ISS and granite models are static URDFs that are included in the media distribution. This design choice enables us to load these entities from a world file, preventing race conditions in loading environmental elements with free-flyers in a simulated context. The way URDF works is best illustrated by this example, which is actually loosely representative of how the project's launch files actually work: - - - - - - - - +```xml + + + + + + +``` Line (1) specifies the namespace on which the robot will operate. Line (2) creates the namespace. Line (3) invokes xacro to write URDF to parameter /honey/robot_description. diff --git a/doc/general_documentation/INSTALL.md b/doc/general_documentation/INSTALL.md index 4d73b9545d..b2d69fcb16 100644 --- a/doc/general_documentation/INSTALL.md +++ b/doc/general_documentation/INSTALL.md @@ -31,6 +31,7 @@ At this point you need to decide where you'd like to put the source code First, clone the flight software repository and media: git clone https://github.com/nasa/astrobee.git $SOURCE_PATH + pushd $SOURCE_PATH git submodule update --init --depth 1 description/media @@ -46,12 +47,14 @@ Clone the android repository: git clone https://github.com/nasa/astrobee_android.git $ANDROID_PATH -### Dependencies Ubuntu 16 + ROS Melodic +### Dependencies Next, install all required dependencies: + *Note: `root` access is necessary to install the compiled debian packages below* + *Note: Before running this please ensure that your system is completely updated - by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* +by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* pushd $SOURCE_PATH cd scripts/setup @@ -60,50 +63,11 @@ Next, install all required dependencies: cd debians ./build_install_debians.sh cd ../ - ./install_desktop_16_04_packages.sh + ./install_desktop_packages.sh sudo rosdep init rosdep update popd -**Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled. - - E: Unable to locate package libroyale1 - E: Unable to locate package rti - E: Unable to locate package libmiro0 - E: Unable to locate package libsoracore1 - E: Unable to locate package libroyale-dev - E: Unable to locate package rti-dev - E: Unable to locate package libsoracore-dev - E: Unable to locate package libmiro-dev - -### Dependencies Ubuntu 18 + ROS melodic - -Next, install all required dependencies: -*Note: `root` access is necessary to install the compiled debian packages below* -*Note: You should instal ROS melodic beforehand following the [Installation Instructions](http://wiki.ros.org/melodic/Installation/Ubuntu)* -*Note: Before running this please ensure that your system is completely updated - by running 'sudo apt-get update' and then 'sudo apt-get upgrade'* - - pushd $SOURCE_PATH - cd scripts/setup - ./add_ros_repository.sh - sudo apt-get update - -Install OpenCV 3.3.1, you can parse the argument -p to change install directory. By default, install directory is in /usr/local. If you do so, make sure CMake can find it. - - ./install_opencv.sh - -Install Luajit, it will be installed in /usr/local - - ./install_luajit.sh - - cd debians - ./build_install_debians_18_04.sh - cd ../ - ./install_desktop_18_04_packages.sh - sudo rosdep init - rosdep update - popd **Important**: you can safely ignore the following error messages, as they are simply letting you know that certain libraries cannot be found. These libraries are for internal NASA use only, and are not required by public users provided that software is launched with DDS disabled. @@ -116,7 +80,6 @@ Install Luajit, it will be installed in /usr/local E: Unable to locate package libsoracore-dev E: Unable to locate package libmiro-dev - ## Configuring the build ### Note for the build setup @@ -169,5 +132,10 @@ rebuilt, and not the entire code base. If you configured your virtual machine with more than the baseline resources, you can adjust the number of threads (eg. -j4) to speed up the build. +## Cross Compiling + +Please contact your Astrobee point of contact if you need to cross compile the +code. + For more information on running the simulator and moving the robot, please see the [simulation instructions](simulation/sim_overview.md). diff --git a/doc/general_documentation/NASA_INSTALL.md b/doc/general_documentation/NASA_INSTALL.md index c83ea761c4..1e7ad29b18 100644 --- a/doc/general_documentation/NASA_INSTALL.md +++ b/doc/general_documentation/NASA_INSTALL.md @@ -83,12 +83,12 @@ First, clone the flight software repository: git clone https://github.com/nasa/astrobee.git --branch develop $SOURCE_PATH git submodule update --init --depth 1 description/media + git submodule update --init --depth 1 submodules/platform You can either choose which optional submodules to clone and log depth with: git submodule update --init --depth 1 submodules/android git submodule update --init --depth 1 submodules/avionics - git submodule update --init --depth 1 submodules/platform Or checkout all the submodules as: @@ -99,14 +99,20 @@ The android module is necessary for guest science code; the avionics and platfor module is used when cross-compiling to test on the robot hardware. ### Dependencies +For *Ubuntu 18 only*: install openCV and Luajit beforehand with: + + pushd $SOURCE_PATH + cd scripts/setup + ./debians/install_luajit.sh + ./debians/install_opencv.sh -Next, install all required dependencies: +Next, install all remaining dependencies dependencies: pushd $SOURCE_PATH cd scripts/setup ./add_local_repository.sh ./add_ros_repository.sh - ./install_desktop_16_04_packages.sh + ./install_desktop_packages.sh popd #### Extra options to install the dependencies @@ -117,11 +123,9 @@ dependencies, you can use the `NDC_USERNAME` variable. If you prefer to install them at a different location, you can use the `ARS_DEB_DIR` variable. -``` -export NDC_USERNAME=jdoe -export ARS_DEB_DIR=$HOME/astrobee_debs -./add_local_repository.sh -``` + export NDC_USERNAME=jdoe + export ARS_DEB_DIR=$HOME/astrobee_debs + ./add_local_repository.sh ### Cross-compile setup diff --git a/doc/general_documentation/adding_a_command.md b/doc/general_documentation/adding_a_command.md index cc9e8a7e6e..4aa1a2f60e 100644 --- a/doc/general_documentation/adding_a_command.md +++ b/doc/general_documentation/adding_a_command.md @@ -2,49 +2,84 @@ # Introduction -Adding a ground command to the flight software is a pretty involved task so please make sure you have a full understanding of the system before you attempt it. There are four parts to adding a command: adding the command to the command spreedsheet, adding the command to the plan schema, generating the fsw and Android files, and adding the command to the executive. Please make sure to complete all four parts to ensure your command is fully intergrated into the system. +Adding a ground command to the flight software is a pretty involved task so +please make sure you have a full understanding of the system before you attempt +it. There are four parts to adding a command: adding the command to the command +spreedsheet, adding the command to the plan schema, generating the fsw and +Android files, and adding the command to the executive. Please make sure to +complete all four parts to ensure your command is fully intergrated into the +system. # Update the Commands Spreadsheet -The command spreadsheet can be found in freeflyer_docs/FlightSoftware and is named commands.xml. This spreadsheet keeps track of the ground commands the fsw accepts, which commands can run while other commands are running, the operating and mobility states the commands are accepted in, whether the command is accept in a plan or not, and the status of the command in the fsw. Please make sure to add your command to each of the relevant sheets. If you have questions, please contact Katie Hamilton. +The command spreadsheet can be found in freeflyer_docs/FlightSoftware and is +named commands.xml. This spreadsheet keeps track of the ground commands the fsw +accepts, which commands can run while other commands are running, the operating +and mobility states the commands are accepted in, whether the command is accept +in a plan or not, and the status of the command in the fsw. Please make sure to +add your command to each of the relevant sheets. If you have questions, please +contact Katie Hamilton. # Add Command to the Plan Schema -The plan schema can be found in astrobee/commands. Please add your command to the freeFlyerPlanSchema.json file. Make sure to folloow the json format for a command. +The plan schema can be found in astrobee/commands. Please add your command to +the freeFlyerPlanSchema.json file. Make sure to follow the json format for a +command. # File Generation -When adding a command, some fsw and GDS files need to be updated. There are scripts that will update everything for you. After adding your command to the plan schema, please follow the next three subsections to generate the files. +When adding a command, some fsw and GDS files need to be updated. There are +scripts that will update everything for you. After adding your command to the +plan schema, please follow the next three subsections to generate the files. ## Setup -To use the scripts, you will need to checkout the xgds2 repo and install some python and geocam tools. +To use the scripts, you will need to checkout the xgds2 repo and install some +python and geocam tools. cd astrobee/commands/ git clone https://github.com/xgds/xgds_planner2.git sudo apt-get install python-pip python-iso8601 sudo pip install git+https://github.com/geocam/geocamUtilWeb -In order to generate the Android files, the xpjson planner parser needs to be changed to keep the parent field for each command parameter. To do this, open the astrobee/commands/xgds_planner2/xgds_planner2/xpjson.py file in your preferred text editor. Find the KEEP_PARAM_PARENT parameter (around line 114) and set it to true. Then save and close the file. +In order to generate the Android files, the xpjson planner parser needs to be +changed to keep the parent field for each command parameter. To do this, open +the astrobee/commands/xgds_planner2/xgds_planner2/xpjson.py file in your +preferred text editor. Find the KEEP_PARAM_PARENT parameter (around line 114) +and set it to true. Then save and close the file. ## Generate FSW files -To generate the fsw files and copy them to correct location, please run the following line from the top level directory of astrobee: +To generate the fsw files and copy them to correct location, please run the +following line from the top level directory of astrobee: ./scripts/build/processCommandSchema.sh -After running this script, please make sure your changes compile. To do this, navigate to the folder you build the fsw in, rebuild the cache, and then compile the code. Don't forget to commit and push your changes. If you want your command incorporated into GDS, please let whoever is in charge of GDS know that the command constants idl has changed. +After running this script, please make sure your changes compile. To do this, +navigate to the folder you build the fsw in, rebuild the cache, and then compile +the code. Don't forget to commit and push your changes. If you want your command +incorporated into GDS, please let whoever is in charge of GDS know that the +command constants idl has changed. ## Generate Android files -Make sure you have the Astrobee Android repo. If you don't, please see the install instructions on how to get it. Navigate to the astrobee api scripts folder in the astrobee Android repo (astrobee_api/scripts). These scripts assume that the android repo is located in the fsw submodules folder. If it is located outside of the fsw, please set the $SOURCE_PATH environment variable to the fsw location. Also if you added a new enum command type, you will need to add an include line to both the genBaseRobot and genBaseRobotImpl scripts. To generate the android files, please run the following in the scripts folder: +Make sure you have the Astrobee Android repo. If you don't, please see the +install instructions on how to get it. Navigate to the astrobee api scripts +folder in the astrobee Android repo (astrobee_api/scripts). These scripts assume +that the android repo is located in the fsw submodules folder. If it is located +outside of the fsw, please set the $SOURCE_PATH environment variable to the fsw +location. Also if you added a new enum command type, you will need to add an +include line to both the genBaseRobot and genBaseRobotImpl scripts. To generate +the android files, please run the following in the scripts folder: cd astrobee_api/scripts # if needed, set the source path export SOURCE_PATH=path/to/astrobee ./processCommandSchema.sh -The script will copy the generated files to the correct locations. Please make sure that the astrobee_api builds with no errors. To do this, go to the top level astrobee api directory and build the code: +The script will copy the generated files to the correct locations. Please make +sure that the astrobee_api builds with no errors. To do this, go to the top +level astrobee api directory and build the code: cd astrobee_api/ ./gradlew build @@ -54,7 +89,6 @@ Once everything compiles, make sure you commit and push your changes. # Add to Executive -The last step is to add the command to the executive. Please make sure you have a full understanding of the executive and operating state classes before adding the command. If you have questions, please contact Katie Hamilton. - - - +The last step is to add the command to the executive. Please make sure you have +a full understanding of the executive and operating state classes before adding +the command. If you have questions, please contact Katie Hamilton. diff --git a/gnc/ctl/test/test_ctl.test b/gnc/ctl/test/test_ctl.test index d7391d150c..b5383985ef 100644 --- a/gnc/ctl/test/test_ctl.test +++ b/gnc/ctl/test/test_ctl.test @@ -19,6 +19,5 @@ - diff --git a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc index 2fb25e5648..da46bda40e 100644 --- a/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc +++ b/hardware/pmc_actuator/src/pmc_actuator_nodelet/pmc_actuator_nodelet.cc @@ -37,6 +37,7 @@ // Services #include +#include #include #include @@ -106,6 +107,10 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { srv_ = nh->advertiseService( SERVICE_HARDWARE_PMC_ENABLE, &PmcActuatorNodelet::EnableService, this); + // Update PMC watchdog timer timeout + update_timeout_srv_ = nh->advertiseService( + SERVICE_HARDWARE_PMC_TIMOUT, &PmcActuatorNodelet::IdlingTimoutService, this); + // Watchdog timer timer_ = nh->createTimer( watchdog_period_, &PmcActuatorNodelet::TimerCallback, this, false, true); @@ -189,6 +194,12 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { return false; } + // get minimum allowed control rate + if (!config_params.GetPosReal("max_timeout", &max_timeout_)) { + ROS_FATAL("PMC Actuator: minimum control rate not specified!"); + return false; + } + // get initial fan speed if (!config_params.GetInt("null_speed", &null_fan_speed_)) { ROS_FATAL("PMC Actuator: null fan speed not specified!"); @@ -468,15 +479,33 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { return true; } + // Update Minimum Control Frequency (and cutoff time) + bool IdlingTimoutService(ff_msgs::SetFloat::Request &req, + ff_msgs::SetFloat::Response &res) { // NOLINT + double new_timeout = req.data; + // Check if the new rate is within the safe and default limits + if (new_timeout < max_timeout_ && new_timeout >= (20.0/control_rate_hz_)) { + watchdog_period_ = ros::Duration(new_timeout); + timer_.setPeriod(watchdog_period_); + ROS_INFO("PMC idling timeout updated."); + res.success = true; + } else { + ROS_INFO("Selected timeout is not within the safe timeout bounds."); + res.success = false; + } + return true; + } + private: config_reader::ConfigReader config_params; // LUA configuration reader ros::Subscriber sub_command_; // Command flight mode sub ros::Publisher pub_telemetry_, pub_state_; // Telemetry publisher ros::ServiceServer srv_; // Enable / disable service + ros::ServiceServer update_timeout_srv_; // Update minimum control rate service ros::Timer timer_; // Watchdog timer ros::Duration watchdog_period_; // Watchdog period ff_hw_msgs::PmcTelemetry telemetry_vector_; // Telemetry message - uint32_t num_pmcs_; // Number of PMCs to control + uint32_t num_pmcs_; // Number of PMCs to control int sub_queue_size_; // Subscriber queue size int pub_queue_size_; // Publisher queue size std::string frame_id_; // Frame ID @@ -484,6 +513,7 @@ class PmcActuatorNodelet : public ff_util::FreeFlyerNodelet { std::vector i2c_addrs_; // 7-bit I2C addresses int i2c_retries_; // Number of I2C bus retries double control_rate_hz_; // Control rate in Hz. + double max_timeout_; // Maximum idling timeout allowed int null_fan_speed_; // Initial fan speed. std::vector null_nozzle_positions_; // Initial nozzle positions uint8_t trims_[NUM_PMC][NUM_TRIM][NUM_NOZZLE]; // Trims for each nozzle diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt index 2c6b79981b..7e207b201f 100644 --- a/localization/CMakeLists.txt +++ b/localization/CMakeLists.txt @@ -16,6 +16,7 @@ # under the License. add_subdirectory(camera) +add_subdirectory(graph_optimizer) add_subdirectory(imu_integration) add_subdirectory(interest_point) add_subdirectory(localization_common) @@ -31,7 +32,7 @@ if (USE_ROS) add_subdirectory(handrail_detect) add_subdirectory(localization_manager) add_subdirectory(localization_node) - if (ENABLE_VIVE_SOLVER) + if (ENABLE_VIVE_SOLVER AND ENABLE_VIVE) add_subdirectory(vive_localization) endif() endif (USE_ROS) diff --git a/localization/graph_localizer/CMakeLists.txt b/localization/graph_localizer/CMakeLists.txt index 15eebabd31..fee4dc0d2a 100644 --- a/localization/graph_localizer/CMakeLists.txt +++ b/localization/graph_localizer/CMakeLists.txt @@ -21,14 +21,14 @@ if (USE_ROS) # include ff_nodelet to get ff_util header files since these aren't exposed elsewhere catkin_package( - LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} camera config_reader ff_nodelet imu_integration localization_common localization_measurements msg_conversions + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} camera config_reader ff_nodelet graph_optimizer imu_integration localization_common localization_measurements msg_conversions INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} CATKIN_DEPENDS roscpp DEPENDS gtsam ff_msgs ) create_library(TARGET ${PROJECT_NAME} - LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} gtsam camera config_reader ff_nodelet imu_integration localization_common localization_measurements msg_conversions + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} gtsam camera config_reader ff_nodelet graph_optimizer imu_integration localization_common localization_measurements msg_conversions INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} DEPS ff_msgs ) diff --git a/localization/graph_localizer/include/graph_localizer/graph_values.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h similarity index 67% rename from localization/graph_localizer/include/graph_localizer/graph_values.h rename to localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h index 7bc94ec2d2..5f7a15fae1 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_values.h +++ b/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h @@ -16,11 +16,11 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_GRAPH_VALUES_H_ -#define GRAPH_LOCALIZER_GRAPH_VALUES_H_ +#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ +#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ -#include -#include +#include +#include #include #include #include @@ -39,9 +39,11 @@ namespace graph_localizer { namespace sym = gtsam::symbol_shorthand; -class GraphValues { +class CombinedNavStateGraphValues : public graph_optimizer::GraphValues { public: - explicit GraphValues(const GraphValuesParams& params = GraphValuesParams()); + CombinedNavStateGraphValues( + const CombinedNavStateGraphValuesParams& params = CombinedNavStateGraphValuesParams(), + std::shared_ptr values = std::shared_ptr(new gtsam::Values())); // Add timestamp and keys to timestamp_key_index_map, and values to values bool AddCombinedNavState(const localization_common::CombinedNavState& combined_nav_state, const int key_index); @@ -57,34 +59,23 @@ class GraphValues { boost::optional> LatestBias() const; // Returns the oldest time that will be in graph values once the window is slid using params - boost::optional SlideWindowNewOldestTime() const; + boost::optional SlideWindowNewOldestTime() const final; boost::optional KeyIndex(const localization_common::Time timestamp) const; - void UpdateValues(const gtsam::Values& new_values); - - // TODO(rsoussan): Put this somewhere else? - static gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, - gtsam::NonlinearFactorGraph& graph); - - gtsam::KeyVector OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const; - - void RemoveOldFeatures(const gtsam::KeyVector& old_feature_keys); - int RemoveOldCombinedNavStates(const localization_common::Time oldest_allowed_time); - gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time) const; - - const gtsam::Values& values() const { return values_; } + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final; boost::optional PoseKey(const localization_common::Time timestamp) const; - boost::optional GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const; + boost::optional GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const final; - boost::optional OldestTimestamp() const; + boost::optional OldestTimestamp() const final; - boost::optional LatestTimestamp() const; + boost::optional LatestTimestamp() const final; boost::optional ClosestPoseTimestamp(const localization_common::Time timestamp) const; @@ -112,31 +103,9 @@ class GraphValues { int NumStates() const; - template - boost::optional at(const gtsam::Key& key) const { - if (!values_.exists(key)) { - LogError("at: Key not present in values."); - return boost::none; - } - - return values_.at(key); - } - boost::optional Timestamp(const int key_index) const; - bool HasFeature(const localization_measurements::FeatureId id) const; - - boost::optional FeatureKey(const localization_measurements::FeatureId id) const; - - // TODO(rsoussan): This shouldn't be const, modify when changes are made to projection factor adder - gtsam::Key CreateFeatureKey() const; - - bool AddFeature(const localization_measurements::FeatureId id, const gtsam::Point3& feature_point, - const gtsam::Key& key); - - gtsam::KeyVector FeatureKeys() const; - - int NumFeatures() const; + const CombinedNavStateGraphValuesParams& params() const; private: // Removes keys from timestamp_key_index_map, values from values @@ -151,19 +120,13 @@ class GraphValues { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(graph_optimizer::GraphValues); ar& BOOST_SERIALIZATION_NVP(timestamp_key_index_map_); - ar& BOOST_SERIALIZATION_NVP(feature_id_key_map_); - ar& BOOST_SERIALIZATION_NVP(feature_key_index_); } - GraphValuesParams params_; - gtsam::Values values_; + CombinedNavStateGraphValuesParams params_; std::map timestamp_key_index_map_; - std::unordered_map feature_id_key_map_; - // Modified by projection_factor_adder, remove mutable if this changes - mutable std::uint64_t feature_key_index_; }; } // namespace graph_localizer -#endif // GRAPH_LOCALIZER_GRAPH_VALUES_H_ +#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_values_params.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h similarity index 78% rename from localization/graph_localizer/include/graph_localizer/graph_values_params.h rename to localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h index 62d82c984a..2e186ef7c7 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_values_params.h +++ b/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h @@ -15,20 +15,18 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ -#define GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ +#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ +#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ #include namespace graph_localizer { -struct GraphValuesParams { +struct CombinedNavStateGraphValuesParams { // Only kept if there are at least min_num_states and not more than max_num_states double ideal_duration; int min_num_states; int max_num_states; - // If storing 3d features as states (i.e. bundle adjustment type problems) - int min_num_factors_per_feature; }; } // namespace graph_localizer -#endif // GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ +#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h new file mode 100644 index 0000000000..7b3868c2bb --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h @@ -0,0 +1,99 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ +#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ + +#include +#include +#include +#include +#include +#include + +namespace graph_localizer { +class CombinedNavStateNodeUpdater + : public graph_optimizer::NodeUpdaterWithPriors { + public: + CombinedNavStateNodeUpdater(const CombinedNavStateNodeUpdaterParams& params, + std::shared_ptr latest_imu_integrator, + std::shared_ptr values); + + void AddInitialValuesAndPriors(gtsam::NonlinearFactorGraph& factors); + + void AddInitialValuesAndPriors(const localization_common::CombinedNavState& global_N_body, + const localization_common::CombinedNavStateNoise& noise, + gtsam::NonlinearFactorGraph& factors) final; + + void AddPriors(const localization_common::CombinedNavState& global_N_body, + const localization_common::CombinedNavStateNoise& noise, gtsam::NonlinearFactorGraph& factors) final; + + bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; + + bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) final; + + void ThresholdBiasUncertainty(gtsam::Matrix& bias_covariance) const; + + graph_optimizer::NodeUpdaterType type() const final; + + boost::optional SlideWindowNewOldestTime() const final; + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final; + + boost::optional GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const final; + + boost::optional OldestTimestamp() const final; + + boost::optional LatestTimestamp() const final; + + std::shared_ptr shared_graph_values() const; + + std::shared_ptr shared_graph_values(); + + const CombinedNavStateGraphValues& graph_values() const; + + private: + void RemovePriors(const int key_index, gtsam::NonlinearFactorGraph& factors); + int GenerateKeyIndex(); + bool AddOrSplitImuFactorIfNeeded(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values); + bool CreateAndAddLatestImuFactorAndCombinedNavState(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values); + bool CreateAndAddImuFactorAndPredictedCombinedNavState(const localization_common::CombinedNavState& global_N_body, + const gtsam::PreintegratedCombinedMeasurements& pim, + gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values); + bool SplitOldImuFactorAndAddCombinedNavState(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values); + + CombinedNavStateNodeUpdaterParams params_; + std::shared_ptr latest_imu_integrator_; + std::shared_ptr graph_values_; + int key_index_; + localization_common::CombinedNavStateNoise global_N_body_start_noise_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/noise_params.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h similarity index 62% rename from localization/graph_localizer/include/graph_localizer/noise_params.h rename to localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h index 55c919cfe5..654bcb5464 100644 --- a/localization/graph_localizer/include/graph_localizer/noise_params.h +++ b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h @@ -15,20 +15,27 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_NOISE_PARAMS_H_ -#define GRAPH_LOCALIZER_NOISE_PARAMS_H_ +#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ +#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ -#include +#include +#include namespace graph_localizer { -struct NoiseParams { +struct CombinedNavStateNodeUpdaterParams { double starting_prior_translation_stddev; double starting_prior_quaternion_stddev; double starting_prior_velocity_stddev; double starting_prior_accel_bias_stddev; double starting_prior_gyro_bias_stddev; - double point_prior_translation_stddev; + double huber_k; + localization_common::CombinedNavState global_N_body_start; + bool add_priors; + CombinedNavStateGraphValuesParams graph_values; + bool threshold_bias_uncertainty; + double accel_bias_stddev_threshold; + double gyro_bias_stddev_threshold; }; } // namespace graph_localizer -#endif // GRAPH_LOCALIZER_NOISE_PARAMS_H_ +#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h b/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h new file mode 100644 index 0000000000..86eb6af8a7 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h @@ -0,0 +1,93 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ +#define GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace graph_localizer { +namespace sym = gtsam::symbol_shorthand; +// TODO(rsoussan): Make seperate base class for static graph values so don't have to return boost::none +// for so many virtual fcns +class FeaturePointGraphValues : public graph_optimizer::GraphValues { + public: + FeaturePointGraphValues(std::shared_ptr values = std::shared_ptr(new gtsam::Values())); + + // Returns the oldest time that will be in graph values once the window is slid using params + boost::optional SlideWindowNewOldestTime() const final; + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final; + + boost::optional GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const final; + + boost::optional OldestTimestamp() const final; + + boost::optional LatestTimestamp() const final; + + gtsam::KeyVector OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const; + + void RemoveOldFeatures(const gtsam::KeyVector& old_feature_keys); + + gtsam::KeyVector FeatureKeys() const; + + int NumFeatures() const; + + bool AddFeature(const localization_measurements::FeatureId id, const gtsam::Point3& feature_point, + const gtsam::Key& key); + + bool HasFeature(const localization_measurements::FeatureId id) const; + + boost::optional FeatureKey(const localization_measurements::FeatureId id) const; + + // TODO(rsoussan): This shouldn't be const, modify when changes are made to projection factor adder + gtsam::Key CreateFeatureKey() const; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(feature_id_key_map_); + ar& BOOST_SERIALIZATION_NVP(feature_key_index_); + } + + std::unordered_map feature_id_key_map_; + // Modified by projection_factor_adder, remove mutable if this changes + mutable std::uint64_t feature_key_index_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h b/localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h new file mode 100644 index 0000000000..4928d6f8a5 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ +#define GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ + +#include +#include +#include +#include + +namespace graph_localizer { +class FeaturePointNodeUpdater + : public graph_optimizer::NodeUpdaterWithPriors { + public: + FeaturePointNodeUpdater(const FeaturePointNodeUpdaterParams& params, std::shared_ptr values); + void AddInitialValuesAndPriors(const localization_common::FeaturePoint3d& global_N_body, + const localization_common::FeaturePoint3dNoise& noise, + gtsam::NonlinearFactorGraph& factors) final; + + void AddPriors(const localization_common::FeaturePoint3d& global_N_body, + const localization_common::FeaturePoint3dNoise& noise, gtsam::NonlinearFactorGraph& factors) final; + + bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; + + bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) final; + + void UpdatePointPriors(const gtsam::Marginals& marginals, gtsam::NonlinearFactorGraph& factors); + + graph_optimizer::NodeUpdaterType type() const final; + + boost::optional SlideWindowNewOldestTime() const final; + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final; + + boost::optional GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const final; + + boost::optional OldestTimestamp() const final; + + boost::optional LatestTimestamp() const final; + + int NumFeatures() const; + + std::shared_ptr shared_feature_point_graph_values(); + + const FeaturePointGraphValues& feature_point_graph_values() const; + + private: + FeaturePointNodeUpdaterParams params_; + std::shared_ptr feature_point_graph_values_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_action.h b/localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h similarity index 73% rename from localization/graph_localizer/include/graph_localizer/graph_action.h rename to localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h index 3b586063b3..1aca866e0d 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_action.h +++ b/localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h @@ -15,18 +15,15 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ +#define GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ -#ifndef GRAPH_LOCALIZER_GRAPH_ACTION_H_ -#define GRAPH_LOCALIZER_GRAPH_ACTION_H_ +#include namespace graph_localizer { -enum class GraphAction { - kNone, - kDeleteExistingSmartFactors, - kLocProjectionNoiseScaling, - kARTagProjectionNoiseScaling, - kTriangulateNewPoint +struct FeaturePointNodeUpdaterParams { + double huber_k; }; } // namespace graph_localizer -#endif // GRAPH_LOCALIZER_GRAPH_ACTION_H_ +#endif // GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_track.h b/localization/graph_localizer/include/graph_localizer/feature_track.h index 39441aad52..55f8ca33d7 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_track.h +++ b/localization/graph_localizer/include/graph_localizer/feature_track.h @@ -21,28 +21,48 @@ #include -#include +#include +#include #include namespace graph_localizer { - -struct FeatureTrack { - localization_measurements::FeatureId id; - std::deque points; - localization_measurements::ImageId latest_image_id; +class FeatureTrack { + public: + using Points = std::map; + explicit FeatureTrack(const localization_measurements::FeatureId id); + FeatureTrack() {} + void AddMeasurement(const localization_common::Time timestamp, + const localization_measurements::FeaturePoint& feature_point); + void RemoveOldMeasurements(const localization_common::Time oldest_allowed_timestamp); + bool HasMeasurement(const localization_common::Time timestamp); + const Points& points() const; + const localization_measurements::FeatureId& id() const; + size_t size() const; + bool empty() const; + std::vector AllowedPoints( + const std::set& allowed_timestamps) const; + std::vector LatestPointsInWindow(const double duration) const; + std::vector LatestPoints(const int spacing = 0) const; + bool SpacingFits(const int spacing, const int max_num_points) const; + int MaxSpacing(const int max_num_points) const; + int ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const; + boost::optional LatestPoint() const; + boost::optional PreviousTimestamp() const; + boost::optional LatestTimestamp() const; + boost::optional OldestTimestamp() const; private: // Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(id); - ar& BOOST_SERIALIZATION_NVP(latest_image_id); - ar& BOOST_SERIALIZATION_NVP(points); + ar& BOOST_SERIALIZATION_NVP(id_); + ar& BOOST_SERIALIZATION_NVP(points_); } -}; -using FeatureTracks = std::vector; + localization_measurements::FeatureId id_; + Points points_; +}; } // namespace graph_localizer #endif // GRAPH_LOCALIZER_FEATURE_TRACK_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_tracker.h b/localization/graph_localizer/include/graph_localizer/feature_tracker.h index 606aa31537..44c6af1f53 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_tracker.h +++ b/localization/graph_localizer/include/graph_localizer/feature_tracker.h @@ -28,20 +28,34 @@ #include #include +#include namespace graph_localizer { -using FeatureTrackMap = std::map; +using FeatureTrackIdMap = std::map>; +using FeatureTrackLengthMap = std::multimap>; class FeatureTracker { public: explicit FeatureTracker(const FeatureTrackerParams& params = FeatureTrackerParams()); // Update existing tracks and add new tracks. Remove tracks without // detections. void UpdateFeatureTracks(const localization_measurements::FeaturePoints& feature_points); - const FeatureTrackMap& feature_tracks() const { return feature_tracks_; } - void RemovePointsOutsideWindow(); - void RemoveOldFeaturePoints(localization_common::Time oldest_allowed_time); + const FeatureTrackIdMap& feature_tracks() const; + const std::set& smart_factor_timestamp_allow_list() const; + const FeatureTrackLengthMap& feature_tracks_length_ordered() const; + int NumTracksWithAtLeastNPoints(int n) const; + void RemoveUndetectedFeatures(const localization_common::Time& feature_point); + void RemoveOldFeaturePointsAndSlideWindow( + boost::optional oldest_allowed_time = boost::none); + void AddOrUpdateTrack(const localization_measurements::FeaturePoint& feature_point); + void UpdateLengthMap(); + void UpdateAllowList(const localization_common::Time& timestamp); + void SlideAllowList(const localization_common::Time& oldest_allowed_time); + boost::optional LongestFeatureTrack() const; + size_t size() const; + bool empty() const; + void Clear(); boost::optional OldestTimestamp() const; - boost::optional latest_timestamp() const; + boost::optional LatestTimestamp() const; boost::optional PreviousTimestamp() const; private: @@ -49,12 +63,15 @@ class FeatureTracker { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(feature_tracks_); + ar& BOOST_SERIALIZATION_NVP(feature_track_id_map_); + ar& BOOST_SERIALIZATION_NVP(feature_track_length_map_); } - FeatureTrackMap feature_tracks_; + FeatureTrackIdMap feature_track_id_map_; + FeatureTrackLengthMap feature_track_length_map_; FeatureTrackerParams params_; - boost::optional latest_time_; + // TODO(rsoussan): Move ths somewhere else? + std::set smart_factor_timestamp_allow_list_; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h b/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h index e0132786a0..149052c312 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h +++ b/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h @@ -22,6 +22,8 @@ namespace graph_localizer { struct FeatureTrackerParams { // Max duration, feature tracker trims measurements outside of this window or outside of graph window double sliding_window_duration; + int smart_projection_adder_measurement_spacing; + bool use_allowed_timestamps; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer.h index f82c5abe01..acb11382de 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer.h @@ -19,19 +19,22 @@ #ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ -#include +#include +#include #include -#include +#include #include -#include -#include -#include +#include #include #include +#include +#include #include #include #include +#include #include +#include #include #include #include @@ -68,12 +71,12 @@ using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor; using SharedRobustSmartFactor = boost::shared_ptr; using ProjectionFactor = gtsam::GenericProjectionFactor; -class GraphLocalizer { +class GraphLocalizer : public graph_optimizer::GraphOptimizer { public: explicit GraphLocalizer(const GraphLocalizerParams& params); // For Serialization Only GraphLocalizer() {} - ~GraphLocalizer(); + ~GraphLocalizer() = default; void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); boost::optional LatestCombinedNavState() const; boost::optional GetCombinedNavState( @@ -82,22 +85,13 @@ class GraphLocalizer { LatestCombinedNavStateAndCovariances() const; bool AddOpticalFlowMeasurement( const localization_measurements::FeaturePointsMeasurement& optical_flow_feature_points_measurement); - bool TriangulateNewPoint(FactorsToAdd& factors_to_add); - bool LocProjectionNoiseScaling(FactorsToAdd& factors_to_add); - bool ARProjectionNoiseScaling(FactorsToAdd& factors_to_add); - bool MapProjectionNoiseScaling(const LocFactorAdderParams& params, FactorsToAdd& factors_to_add); - void CheckForStandstill( - const localization_measurements::FeaturePointsMeasurement& optical_flow_feature_points_measurement); + void CheckForStandstill(); void AddARTagMeasurement( const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); void AddSparseMappingMeasurement( const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); - // Attempts to remove most recent or oldest measurements to make and invalid smart factor valid - // TODO(rsoussan): Move this to SmartProjectionFactorAdder or utilities! - void SplitSmartFactorsIfNeeded(FactorsToAdd& factors_to_add); - - bool Update(); - const FeatureTrackMap& feature_tracks() const { return feature_tracker_->feature_tracks(); } + bool DoPostOptimizeActions() final; + const FeatureTrackIdMap& feature_tracks() const { return feature_tracker_->feature_tracks(); } boost::optional> LatestBiases() const; @@ -107,154 +101,69 @@ class GraphLocalizer { int NumOFFactors(const bool check_valid = true) const; - int NumSmartFactors(const bool check_valid = true) const; - int NumProjectionFactors(const bool check_valid = true) const; - const GraphValues& graph_values() const; - - const gtsam::NonlinearFactorGraph& factor_graph() const; - - void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; - bool standstill() const; const GraphLocalizerParams& params() const; - template - int NumFactors() const { - int num_factors = 0; - for (const auto& factor : graph_) { - if (dynamic_cast(factor.get())) { - ++num_factors; - } - } - return num_factors; - } - - void LogOnDestruction(const bool log_on_destruction); - - const GraphStats& graph_stats() const; + const GraphLocalizerStats& graph_localizer_stats() const; void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode); const localization_measurements::FanSpeedMode fan_speed_mode() const; private: - gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors, - const gtsam::KeyVector& old_keys, - const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const; - - // Removes Keys and Values outside of sliding window. - // Removes any factors depending on removed values - // Optionally adds marginalized factors encapsulating linearized error of removed factors - // Optionally adds priors using marginalized covariances for new oldest states - bool SlideWindow(const boost::optional& marginals, - const localization_common::Time last_latest_time); - - void UpdatePointPriors(const gtsam::Marginals& marginals); - - void RemovePriors(const int key_index); - - // Integrates latest imu measurements up to timestamp and adds imu factor and - // new combined nav state - bool CreateAndAddLatestImuFactorAndCombinedNavState(const localization_common::Time timestamp); - - bool AddOrSplitImuFactorIfNeeded(const localization_common::Time timestamp); - - bool SplitOldImuFactorAndAddCombinedNavState(const localization_common::Time timestamp); - - void AddStartingPriors(const localization_common::CombinedNavState& global_N_body_start, const int key_index, - gtsam::NonlinearFactorGraph& graph); - - void AddPriors(const localization_common::CombinedNavState& global_N_body, - const localization_common::CombinedNavStateNoise& noise, const int key_index, - gtsam::NonlinearFactorGraph& graph); + void DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, + const boost::optional& marginals) final; boost::optional> LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const; - bool CreateAndAddImuFactorAndPredictedCombinedNavState(const localization_common::CombinedNavState& global_N_body, - const gtsam::PreintegratedCombinedMeasurements& pim); - - void BufferFactors(const std::vector& factors_to_add_vec); - - void BufferCumulativeFactors(); - - void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys); - - int AddBufferedFactors(); + void BufferCumulativeFactors() final; - bool DoGraphAction(FactorsToAdd& factors_to_add); + void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) final; - bool Rekey(FactorToAdd& factor_to_add); + bool ValidGraph() const final; - bool ReadyToAddMeasurement(const localization_common::Time timestamp) const; + bool ReadyToAddFactors(const localization_common::Time timestamp) const final; - bool MeasurementRecentEnough(const localization_common::Time timestamp) const; + bool MeasurementRecentEnough(const localization_common::Time timestamp) const final; - void RemoveOldBufferedFactors(const localization_common::Time oldest_allowed_timestamp); - - std::vector TimestampsToAdd(const localization_common::Time timestamp, - const localization_common::Time last_added_timestamp); - - template - void DeleteFactors() { - int num_removed_factors = 0; - for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { - if (dynamic_cast(factor_it->get())) { - factor_it = graph_.erase(factor_it); - ++num_removed_factors; - continue; - } - ++factor_it; - } - LogDebug("DeleteFactors: Num removed factors: " << num_removed_factors); - } - - // TODO(rsoussan): make a static and dynamic key index? - static int GenerateKeyIndex() { - static int key_index = 0; - return key_index++; - } - - void PrintFactorDebugInfo() const; + void PrintFactorDebugInfo() const final; // Serialization function friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(graph_optimizer::GraphOptimizer); ar& BOOST_SERIALIZATION_NVP(feature_tracker_); - ar& BOOST_SERIALIZATION_NVP(graph_); - ar& BOOST_SERIALIZATION_NVP(graph_values_); } std::shared_ptr feature_tracker_; - std::shared_ptr standstill_feature_tracker_; - imu_integration::LatestImuIntegrator latest_imu_integrator_; - std::shared_ptr graph_values_; - bool log_on_destruction_; + std::shared_ptr latest_imu_integrator_; GraphLocalizerParams params_; - gtsam::LevenbergMarquardtParams levenberg_marquardt_params_; - gtsam::TriangulationParameters projection_triangulation_params_; - gtsam::SmartProjectionParams smart_projection_params_; - gtsam::NonlinearFactorGraph graph_; - boost::optional marginals_; boost::optional last_optical_flow_measurement_; - std::multimap buffered_factors_to_add_; // Factor Adders - std::unique_ptr ar_tag_loc_factor_adder_; - std::unique_ptr loc_factor_adder_; - std::unique_ptr projection_factor_adder_; - std::unique_ptr rotation_factor_adder_; - std::unique_ptr smart_projection_cumulative_factor_adder_; - std::unique_ptr standstill_factor_adder_; - - gtsam::Marginals::Factorization marginals_factorization_; + std::shared_ptr ar_tag_loc_factor_adder_; + std::shared_ptr loc_factor_adder_; + std::shared_ptr projection_factor_adder_; + std::shared_ptr rotation_factor_adder_; + std::shared_ptr smart_projection_cumulative_factor_adder_; + std::shared_ptr standstill_factor_adder_; + + // Node Updaters + std::shared_ptr combined_nav_state_node_updater_; + std::shared_ptr feature_point_node_updater_; + + // Graph Action Completers + std::shared_ptr ar_tag_loc_graph_action_completer_; + std::shared_ptr loc_graph_action_completer_; + std::shared_ptr projection_graph_action_completer_; + std::shared_ptr smart_projection_graph_action_completer_; + boost::optional standstill_; - boost::optional last_latest_time_; - GraphStats graph_stats_; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h index 9f6dd7db2d..ec6d197647 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h @@ -19,40 +19,30 @@ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ #include +#include #include +#include #include #include -#include -#include +#include #include #include namespace graph_localizer { struct GraphLocalizerParams { + CombinedNavStateNodeUpdaterParams combined_nav_state_node_updater; CalibrationParams calibration; FactorParams factor; + FeaturePointNodeUpdaterParams feature_point_node_updater; FeatureTrackerParams feature_tracker; - FeatureTrackerParams standstill_feature_tracker; - GraphValuesParams graph_values; - NoiseParams noise; + graph_optimizer::GraphOptimizerParams graph_optimizer; GraphInitializerParams graph_initializer; - bool verbose; - bool fatal_failures; - bool print_factor_info; - bool use_ceres_params; - int max_iterations; - std::string marginals_factorization; - bool limit_imu_factor_spacing; - double max_imu_factor_spacing; - bool add_priors; - bool add_marginal_factors; double max_standstill_feature_track_avg_distance_from_mean; int standstill_min_num_points_per_track; double huber_k; - int log_rate; - int optical_flow_measurement_spacing; bool estimate_world_T_dock_using_loc; + double standstill_feature_track_duration; localization_measurements::FanSpeedMode initial_fan_speed_mode; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/graph_stats.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h similarity index 59% rename from localization/graph_localizer/include/graph_localizer/graph_stats.h rename to localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h index b668ffae58..85b5989f27 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_stats.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h @@ -15,44 +15,27 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_GRAPH_STATS_H_ -#define GRAPH_LOCALIZER_GRAPH_STATS_H_ +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ -#include -#include - -#include +#include +#include namespace graph_localizer { -// Forward declaration since GraphLocalizer will have a GraphStats object -class GraphLocalizer; - -class GraphStats { +class GraphLocalizerStats : public graph_optimizer::GraphStats { public: - GraphStats(); - void UpdateErrors(const GraphLocalizer& graph); - void UpdateStats(const GraphLocalizer& graph); - void Log() const; - void LogToFile(std::ofstream& ofstream) const; - void LogToCsv(std::ofstream& ofstream) const; - - std::vector> timers_; - std::vector> stats_averagers_; - std::vector> error_averagers_; - - // Timers - localization_common::Timer optimization_timer_ = localization_common::Timer("Optimization"); - localization_common::Timer update_timer_ = localization_common::Timer("Update"); - localization_common::Timer marginals_timer_ = localization_common::Timer("Marginals"); - localization_common::Timer slide_window_timer_ = localization_common::Timer("Slide Window"); - localization_common::Timer add_buffered_factors_timer_ = localization_common::Timer("Add Buffered Factors"); - localization_common::Timer log_error_timer_ = localization_common::Timer("Log Error"); - localization_common::Timer log_stats_timer_ = localization_common::Timer("Log Stats"); + GraphLocalizerStats(); + void SetCombinedNavStateGraphValues( + std::shared_ptr combined_nav_state_graph_values); + void UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) final; + void UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) final; // Graph Stats Averagers - localization_common::Averager iterations_averager_ = localization_common::Averager("Iterations"); localization_common::Averager num_states_averager_ = localization_common::Averager("Num States"); localization_common::Averager duration_averager_ = localization_common::Averager("Duration"); + localization_common::Averager num_marginal_factors_averager_ = localization_common::Averager("Num Marginal Factors"); + localization_common::Averager num_factors_averager_ = localization_common::Averager("Num Factors"); + localization_common::Averager num_features_averager_ = localization_common::Averager("Num Features"); localization_common::Averager num_optical_flow_factors_averager_ = localization_common::Averager("Num Optical Flow Factors"); localization_common::Averager num_loc_proj_factors_averager_ = localization_common::Averager("Num Loc Proj Factors"); @@ -63,11 +46,7 @@ class GraphStats { localization_common::Averager("Num Standstill Between Factors"); localization_common::Averager num_vel_prior_factors_averager_ = localization_common::Averager("Num Vel Prior Factors"); - localization_common::Averager num_marginal_factors_averager_ = localization_common::Averager("Num Marginal Factors"); - localization_common::Averager num_factors_averager_ = localization_common::Averager("Num Factors"); - localization_common::Averager num_features_averager_ = localization_common::Averager("Num Features"); // Factor Error Averagers - localization_common::Averager total_error_averager_ = localization_common::Averager("Total Factor Error"); localization_common::Averager of_error_averager_ = localization_common::Averager("OF Factor Error"); localization_common::Averager loc_proj_error_averager_ = localization_common::Averager("Loc Proj Factor Error"); localization_common::Averager loc_pose_error_averager_ = localization_common::Averager("Loc Pose Factor Error"); @@ -80,21 +59,8 @@ class GraphStats { localization_common::Averager bias_prior_error_averager_ = localization_common::Averager("Bias Prior Error"); private: - template - void LogToFile(const std::vector>& loggers, std::ofstream& ofstream) const { - for (const auto& logger : loggers) logger.get().LogToFile(ofstream); - } - - template - void LogToCsv(const std::vector>& loggers, std::ofstream& ofstream) const { - for (const auto& logger : loggers) logger.get().LogToCsv(ofstream); - } - - template - void Log(const std::vector>& loggers) const { - for (const auto& logger : loggers) logger.get().Log(); - } + std::shared_ptr combined_nav_state_graph_values_; }; } // namespace graph_localizer -#endif // GRAPH_LOCALIZER_GRAPH_STATS_H_ +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h index 6fe520f6d3..dedeffda9f 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,7 +79,9 @@ class GraphLocalizerWrapper { void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); - boost::optional feature_tracks() const; + boost::optional feature_tracks() const; + + boost::optional graph_localizer() const; void MarkWorldTDockForResettingIfNecessary(); @@ -89,7 +91,7 @@ class GraphLocalizerWrapper { void SaveLocalizationGraphDotFile() const; - boost::optional graph_stats() const; + boost::optional graph_localizer_stats() const; bool publish_localization_graph() const; diff --git a/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h b/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h index 1e10952124..ce5993c445 100644 --- a/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h +++ b/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h @@ -19,28 +19,31 @@ #ifndef GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ #define GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ -#include -#include #include +#include #include #include #include namespace graph_localizer { -class LocFactorAdder - : public FactorAdder { - using Base = FactorAdder; +class LocFactorAdder : public graph_optimizer::FactorAdder { + using Base = + graph_optimizer::FactorAdder; public: - LocFactorAdder(const LocFactorAdderParams& params, const GraphAction projection_graph_action); + LocFactorAdder(const LocFactorAdderParams& params, + const graph_optimizer::GraphActionCompleterType graph_action_completer_type); - std::vector AddFactors( + std::vector AddFactors( const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement) final; private: + graph_optimizer::GraphActionCompleterType type() const; + + graph_optimizer::GraphActionCompleterType graph_action_completer_type_; localization_common::Averager num_landmarks_averager_ = localization_common::Averager("Num Landmarks"); - GraphAction projection_graph_action_; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h index 41ac30c80e..6d3eb124a9 100644 --- a/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h +++ b/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h @@ -19,14 +19,14 @@ #ifndef GRAPH_LOCALIZER_LOC_FACTOR_ADDER_PARAMS_H_ #define GRAPH_LOCALIZER_LOC_FACTOR_ADDER_PARAMS_H_ -#include +#include #include #include #include namespace graph_localizer { -struct LocFactorAdderParams : public FactorAdderParams { +struct LocFactorAdderParams : public graph_optimizer::FactorAdderParams { bool add_pose_priors; bool add_projections; double prior_translation_stddev; diff --git a/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h new file mode 100644 index 0000000000..6f3c8d67fd --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ +#define GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ + +#include +#include +#include +#include + +namespace graph_localizer { +class LocGraphActionCompleter : public graph_optimizer::GraphActionCompleter { + public: + LocGraphActionCompleter(const LocFactorAdderParams& params, + const graph_optimizer::GraphActionCompleterType graph_action_completer_type, + std::shared_ptr graph_values); + + bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; + + graph_optimizer::GraphActionCompleterType type() const final; + + private: + LocFactorAdderParams params_; + graph_optimizer::GraphActionCompleterType graph_action_completer_type_; + std::shared_ptr graph_values_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/parameter_reader.h b/localization/graph_localizer/include/graph_localizer/parameter_reader.h index 380968de64..98ed4124dc 100644 --- a/localization/graph_localizer/include/graph_localizer/parameter_reader.h +++ b/localization/graph_localizer/include/graph_localizer/parameter_reader.h @@ -20,13 +20,14 @@ #include #include +#include +#include #include +#include #include #include #include #include -#include -#include #include #include #include @@ -45,14 +46,16 @@ void LoadSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, SmartProjectionFactorAdderParams& params); void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, StandstillFactorAdderParams& params); void LoadFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params); -void LoadStandstillFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params); -void LoadGraphValuesParams(config_reader::ConfigReader& config, GraphValuesParams& params); void LoadImuIntegrationParams(config_reader::ConfigReader& config, GraphInitializerParams& params); -void LoadNoiseParams(config_reader::ConfigReader& config, NoiseParams& params); void LoadSanityCheckerParams(config_reader::ConfigReader& config, SanityCheckerParams& params); // Loads all params except some (biases and start pose) that are // not loaded from config files void LoadGraphInitializerParams(config_reader::ConfigReader& config, GraphInitializerParams& params); +void LoadCombinedNavStateGraphValuesParams(config_reader::ConfigReader& config, + CombinedNavStateGraphValuesParams& params); +void LoadCombinedNavStateNodeUpdaterParams(config_reader::ConfigReader& config, + CombinedNavStateNodeUpdaterParams& params); +void LoadFeaturePointNodeUpdaterParams(config_reader::ConfigReader& config, FeaturePointNodeUpdaterParams& params); void LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params); void LoadGraphLocalizerNodeletParams(config_reader::ConfigReader& config, GraphLocalizerNodeletParams& params); } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h b/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h index 402802afd1..fe7bd35123 100644 --- a/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h +++ b/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h @@ -19,30 +19,33 @@ #ifndef GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ #define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ -#include +#include #include #include -#include +#include #include +#include + #include namespace graph_localizer { -class ProjectionFactorAdder - : public FactorAdder { - using Base = FactorAdder; +class ProjectionFactorAdder : public graph_optimizer::FactorAdder { + using Base = + graph_optimizer::FactorAdder; public: ProjectionFactorAdder(const ProjectionFactorAdderParams& params, std::shared_ptr feature_tracker, - std::shared_ptr graph_values); + std::shared_ptr feature_point_graph_values); - std::vector AddFactors( + std::vector AddFactors( const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; private: std::shared_ptr feature_tracker_; - std::shared_ptr graph_values_; + std::shared_ptr feature_point_graph_values_; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h index 04b1bb989d..2c15543913 100644 --- a/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h +++ b/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h @@ -19,7 +19,7 @@ #ifndef GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ #define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ -#include +#include #include #include @@ -28,7 +28,7 @@ #include namespace graph_localizer { -struct ProjectionFactorAdderParams : public FactorAdderParams { +struct ProjectionFactorAdderParams : public graph_optimizer::FactorAdderParams { bool enable_EPI; double landmark_distance_threshold; double dynamic_outlier_rejection_threshold; diff --git a/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h new file mode 100644 index 0000000000..5ff8cecb1d --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ +#define GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ + +#include +#include +#include +#include + +#include + +namespace graph_localizer { +class ProjectionGraphActionCompleter : public graph_optimizer::GraphActionCompleter { + public: + ProjectionGraphActionCompleter(const ProjectionFactorAdderParams& params, + std::shared_ptr graph_values, + std::shared_ptr feature_point_graph_values); + + bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; + + graph_optimizer::GraphActionCompleterType type() const final; + + private: + bool TriangulateNewPoint(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors); + + ProjectionFactorAdderParams params_; + std::shared_ptr graph_values_; + std::shared_ptr feature_point_graph_values_; + gtsam::TriangulationParameters projection_triangulation_params_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h b/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h index 4f52300027..faf1955887 100644 --- a/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h +++ b/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h @@ -75,23 +75,23 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactorcomputeJacobiansSVD(F, E0, b, cameras, *(this->point())); - // TODO(rsoussan): This should be here but leads to a degeneracy. It is done like this in gtsam - // for the jacobiansvd factorization but not the hessian factorization -> why? - } // else if (useForRotationOnly(result)) { // Rotation only factor - // Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); - // this->computeJacobiansSVD(F, E0, b, cameras, backProjected); - // } - else { // Empty factor // NOLINT + } else if (useForRotationOnly(result)) { // Rotation only factor + Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); + // Cheirality error can still occur with backprojection + try { + this->computeJacobiansSVD(F, E0, b, cameras, backProjected); + } catch (...) { + return boost::make_shared>(this->keys()); + } + } else { // Empty factor // NOLINT return boost::make_shared>(this->keys()); } return createRegularJacobianFactorSVD(this->keys(), F, E0, b); } bool useForRotationOnly(const gtsam::TriangulationResult& result) const { - if (!rotation_only_fallback_) return false; - // Enable some 'invalid' results as these can still be useful for rotation errors - // return (result.degenerate()); - return (result.behindCamera()); + // Use rotation only for all failure cases + return true; } double error(const Values& values) const override { @@ -99,8 +99,7 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactortotalReprojectionError(this->cameras(values)); const auto result = this->point(); - // TODO(rsoussan): This should be here but leads to a degeneracy (see comment in linearize) - // if (!result.valid() && !useForRotationOnly(result)) return 0.0; + if (!result.valid() && !useForRotationOnly(result)) return 0.0; // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss // expects mahal distance const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; @@ -122,8 +121,7 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactortotalReprojectionError(this->cameras(values), point); - // TODO(rsoussan): This should be here but leads to a degeneracy (see comment in linearize) - // if (!result.valid() && !useForRotationOnly(result)) return 0.0; + if (!point.valid() && !useForRotationOnly(point)) return 0.0; // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss // expects mahal distance const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; @@ -156,6 +154,26 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactorcameras(values); + const auto point = this->triangulateSafe(cameras); + if (point.valid()) { + return true; + } else if (useForRotationOnly(point)) { + Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); + try { + cameras.reprojectionError(backProjected, this->measured()); + } catch (...) { + return false; + } + return true; + } else { + return false; + } + // Shouldn't get here + return false; + } + private: template boost::shared_ptr> createRegularJacobianFactorSVD( @@ -178,7 +196,7 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor reduced_matrices; reduced_matrices.reserve(numKeys); for (size_t k = 0; k < Fblocks.size(); ++k) { diff --git a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h index b33312b27b..f640fe148a 100644 --- a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h +++ b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h @@ -19,22 +19,24 @@ #ifndef GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ #define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ -#include #include #include +#include #include #include namespace graph_localizer { -class RotationFactorAdder - : public FactorAdder { - using Base = FactorAdder; +class RotationFactorAdder : public graph_optimizer::FactorAdder { + using Base = + graph_optimizer::FactorAdder; public: RotationFactorAdder(const RotationFactorAdderParams& params, std::shared_ptr feature_tracker); - std::vector AddFactors(const localization_measurements::FeaturePointsMeasurement& measurement) final; + std::vector AddFactors( + const localization_measurements::FeaturePointsMeasurement& measurement) final; private: std::shared_ptr feature_tracker_; diff --git a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h index 375b63944b..2b0abfd317 100644 --- a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h +++ b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h @@ -19,13 +19,13 @@ #ifndef GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ #define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ -#include +#include #include #include namespace graph_localizer { -struct RotationFactorAdderParams : public FactorAdderParams { +struct RotationFactorAdderParams : public graph_optimizer::FactorAdderParams { double min_avg_disparity; double rotation_stddev; double max_percent_outliers; diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h b/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h index f34c97bcee..1101fdf4e1 100644 --- a/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h +++ b/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h @@ -19,26 +19,43 @@ #ifndef GRAPH_LOCALIZER_SMART_PROJECTION_CUMULATIVE_FACTOR_ADDER_H_ #define GRAPH_LOCALIZER_SMART_PROJECTION_CUMULATIVE_FACTOR_ADDER_H_ -#include #include #include +#include #include +#include #include namespace graph_localizer { -class SmartProjectionCumulativeFactorAdder : public CumulativeFactorAdder { - using Base = CumulativeFactorAdder; +class SmartProjectionCumulativeFactorAdder + : public graph_optimizer::CumulativeFactorAdder { + using Base = graph_optimizer::CumulativeFactorAdder; public: SmartProjectionCumulativeFactorAdder(const SmartProjectionFactorAdderParams& params, std::shared_ptr feature_tracker); - std::vector AddFactors() final; + std::vector AddFactors() final; + void AddFactors( + const FeatureTrackLengthMap& feature_tracks, const int spacing, const double feature_track_min_separation, + graph_optimizer::FactorsToAdd& smart_factors_to_add, + std::unordered_map& added_points); + void AddAllowedFactors( + const FeatureTrackLengthMap& feature_tracks, const double feature_track_min_separation, + graph_optimizer::FactorsToAdd& smart_factors_to_add, + std::unordered_map& added_points); + + const gtsam::SmartProjectionParams& smart_projection_params() const; private: - void AddSmartFactor(const FeatureTrack& feature_track, FactorsToAdd& smart_factors_to_add) const; + void AddSmartFactor(const std::vector& feature_track_points, + graph_optimizer::FactorsToAdd& smart_factors_to_add) const; + + bool TooClose(const std::unordered_map& + added_points, + const localization_measurements::FeaturePoint& point, const double feature_track_min_separation) const; std::shared_ptr feature_tracker_; gtsam::SmartProjectionParams smart_projection_params_; diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h index a0a4947b2a..5240f70879 100644 --- a/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h +++ b/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h @@ -19,7 +19,7 @@ #ifndef GRAPH_LOCALIZER_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ #define GRAPH_LOCALIZER_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ -#include +#include #include #include @@ -28,7 +28,7 @@ #include namespace graph_localizer { -struct SmartProjectionFactorAdderParams : public FactorAdderParams { +struct SmartProjectionFactorAdderParams : public graph_optimizer::FactorAdderParams { double min_avg_distance_from_mean; bool enable_EPI; double landmark_distance_threshold; @@ -39,10 +39,13 @@ struct SmartProjectionFactorAdderParams : public FactorAdderParams { int max_num_factors; int min_num_points; int max_num_points_per_factor; + int measurement_spacing; + double feature_track_min_separation; bool rotation_only_fallback; bool splitting; bool scale_noise_with_num_points; double noise_scale; + bool use_allowed_timestamps; gtsam::Pose3 body_T_cam; boost::shared_ptr cam_intrinsics; gtsam::SharedIsotropic cam_noise; diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h new file mode 100644 index 0000000000..9e1d89ddc3 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ +#define GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ + +#include +#include +#include + +#include + +namespace graph_localizer { +class SmartProjectionGraphActionCompleter : public graph_optimizer::GraphActionCompleter { + public: + SmartProjectionGraphActionCompleter(const SmartProjectionFactorAdderParams& params, + std::shared_ptr graph_values); + + bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; + + graph_optimizer::GraphActionCompleterType type() const final; + + const gtsam::SmartProjectionParams& smart_projection_params() const; + + private: + void SplitSmartFactorsIfNeeded(const CombinedNavStateGraphValues& graph_values, + graph_optimizer::FactorsToAdd& factors_to_add); + + SmartProjectionFactorAdderParams params_; + std::shared_ptr graph_values_; + gtsam::SmartProjectionParams smart_projection_params_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h index 37a85f6b10..e230f57201 100644 --- a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h +++ b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h @@ -19,23 +19,24 @@ #ifndef GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ #define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ -#include #include #include +#include #include #include namespace graph_localizer { -class StandstillFactorAdder - : public FactorAdder { - using Base = FactorAdder; +class StandstillFactorAdder : public graph_optimizer::FactorAdder { + using Base = + graph_optimizer::FactorAdder; public: explicit StandstillFactorAdder(const StandstillFactorAdderParams& params, std::shared_ptr feature_tracker); - std::vector AddFactors( + std::vector AddFactors( const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; private: diff --git a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h index 2128af5b97..1bd7689710 100644 --- a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h +++ b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h @@ -19,10 +19,10 @@ #ifndef GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ #define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ -#include +#include namespace graph_localizer { -struct StandstillFactorAdderParams : public FactorAdderParams { +struct StandstillFactorAdderParams : public graph_optimizer::FactorAdderParams { bool add_velocity_prior; bool add_pose_between_factor; double prior_velocity_stddev; diff --git a/localization/graph_localizer/include/graph_localizer/utilities.h b/localization/graph_localizer/include/graph_localizer/utilities.h index 03919c446e..8b45a092d1 100644 --- a/localization/graph_localizer/include/graph_localizer/utilities.h +++ b/localization/graph_localizer/include/graph_localizer/utilities.h @@ -22,10 +22,12 @@ #include #include #include +#include #include +#include #include #include -#include +#include #include #include #include @@ -47,11 +49,10 @@ #include namespace graph_localizer { -bool ValidPointSet(const std::deque& points, - const double average_distance_from_mean, const double min_avg_distance_from_mean, - const int min_num_points); +bool ValidPointSet(const int num_points, const double average_distance_from_mean, + const double min_avg_distance_from_mean, const int min_num_points); -double AverageDistanceFromMean(const std::deque& points); +double AverageDistanceFromMean(const std::vector& points); bool ValidVLMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg, const int min_num_landmarks); @@ -59,7 +60,7 @@ ff_msgs::GraphState GraphStateMsg(const localization_common::CombinedNavState& c const localization_common::CombinedNavStateCovariances& covariances, const FeatureCounts& detected_feature_counts, const bool estimating_bias, const double position_log_det_threshold, const double orientation_log_det_threshold, - const bool standstill, const GraphStats& graph_stats, + const bool standstill, const GraphLocalizerStats& graph_stats, const localization_measurements::FanSpeedMode fan_speed_mode); ff_msgs::LocalizationGraph GraphMsg(const GraphLocalizer& graph_localizer); @@ -73,17 +74,20 @@ geometry_msgs::PoseStamped PoseMsg(const gtsam::Pose3& global_T_body, const loca gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k); boost::optional FixSmartFactorByRemovingIndividualMeasurements( - const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values); + const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values); boost::optional FixSmartFactorByRemovingMeasurementSequence( - const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values); + const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values); SharedRobustSmartFactor RemoveSmartFactorMeasurements(const RobustSmartFactor& smart_factor, const std::unordered_set& factor_key_indices_to_remove, const SmartProjectionFactorAdderParams& params, const gtsam::SmartProjectionParams& smart_projection_params); + +int NumSmartFactors(const gtsam::NonlinearFactorGraph& graph_factors, const gtsam::Values& values, + const bool check_valid); } // namespace graph_localizer #endif // GRAPH_LOCALIZER_UTILITIES_H_ diff --git a/localization/graph_localizer/src/graph_values.cc b/localization/graph_localizer/src/combined_nav_state_graph_values.cc similarity index 64% rename from localization/graph_localizer/src/graph_values.cc rename to localization/graph_localizer/src/combined_nav_state_graph_values.cc index 18fdca5625..31067d0c0f 100644 --- a/localization/graph_localizer/src/graph_values.cc +++ b/localization/graph_localizer/src/combined_nav_state_graph_values.cc @@ -16,7 +16,7 @@ * under the License. */ -#include +#include #include #include @@ -29,14 +29,19 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lc = localization_common; namespace lm = localization_measurements; -GraphValues::GraphValues(const GraphValuesParams& params) : params_(params), feature_key_index_(0) { - LogDebug("GraphValues: Window duration: " << params_.ideal_duration); - LogDebug("GraphValues: Window min num states: " << params_.min_num_states); +CombinedNavStateGraphValues::CombinedNavStateGraphValues(const CombinedNavStateGraphValuesParams& params, + std::shared_ptr values) + : GraphValues(std::move(values)), params_(params) { + LogDebug("CombinedNavStateGraphValues: Window duration: " << params_.ideal_duration); + LogDebug("CombinedNavStateGraphValues: Window min num states: " << params_.min_num_states); } -boost::optional GraphValues::LatestCombinedNavState() const { +const CombinedNavStateGraphValuesParams& CombinedNavStateGraphValues::params() const { return params_; } + +boost::optional CombinedNavStateGraphValues::LatestCombinedNavState() const { if (Empty()) { LogError("LatestCombinedNavState: No combined nav states available."); return boost::none; @@ -46,7 +51,7 @@ boost::optional GraphValues::LatestCombinedNavState() cons return GetCombinedNavState(timestamp); } -boost::optional GraphValues::OldestCombinedNavState() const { +boost::optional CombinedNavStateGraphValues::OldestCombinedNavState() const { if (Empty()) { LogError("OldestCombinedNavState: No combined nav states available."); return boost::none; @@ -55,7 +60,7 @@ boost::optional GraphValues::OldestCombinedNavState() cons return GetCombinedNavState(timestamp); } -boost::optional GraphValues::OldestTimestamp() const { +boost::optional CombinedNavStateGraphValues::OldestTimestamp() const { if (Empty()) { LogError("OldestTimestamp: No states available."); return boost::none; @@ -63,7 +68,7 @@ boost::optional GraphValues::OldestTimestamp() const { return timestamp_key_index_map_.cbegin()->first; } -boost::optional GraphValues::LatestTimestamp() const { +boost::optional CombinedNavStateGraphValues::LatestTimestamp() const { if (Empty()) { LogError("LatestTimestamp: No states available."); return boost::none; @@ -71,7 +76,7 @@ boost::optional GraphValues::LatestTimestamp() const { return timestamp_key_index_map_.crbegin()->first; } -boost::optional GraphValues::ClosestPoseTimestamp(const lc::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::ClosestPoseTimestamp(const lc::Time timestamp) const { if (Empty()) { LogError("ClosestPoseTimestamp: No states available."); return boost::none; @@ -100,8 +105,8 @@ boost::optional GraphValues::ClosestPoseTimestamp(const lc::Time times return closest_timestamp; } -std::pair, boost::optional> GraphValues::LowerAndUpperBoundTimestamp( - const lc::Time timestamp) const { +std::pair, boost::optional> +CombinedNavStateGraphValues::LowerAndUpperBoundTimestamp(const lc::Time timestamp) const { if (Empty()) { LogError("LowerAndUpperBoundTimestamp: No states available."); return {boost::none, boost::none}; @@ -122,8 +127,8 @@ std::pair, boost::optional> GraphValues::Low return {lower_bound_it->first, upper_bound_it->first}; } -boost::optional GraphValues::GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::GetKey(go::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const { if (timestamp_key_index_map_.count(timestamp) == 0) { LogError("GetKey: No key index found at timestamp."); return boost::none; @@ -132,7 +137,7 @@ boost::optional GraphValues::GetKey(KeyCreatorFunction key_creator_f const int key_index = timestamp_key_index_map_.at(timestamp); const auto key = key_creator_function(key_index); - if (!values_.exists(key)) { + if (!values().exists(key)) { LogError("GetKey: Key not present in values."); return boost::none; } @@ -140,13 +145,17 @@ boost::optional GraphValues::GetKey(KeyCreatorFunction key_creator_f return key; } -bool GraphValues::HasKey(const lc::Time timestamp) const { return (timestamp_key_index_map_.count(timestamp) != 0); } +bool CombinedNavStateGraphValues::HasKey(const lc::Time timestamp) const { + return (timestamp_key_index_map_.count(timestamp) != 0); +} -bool GraphValues::Empty() const { return timestamp_key_index_map_.empty(); } +bool CombinedNavStateGraphValues::Empty() const { return timestamp_key_index_map_.empty(); } -boost::optional GraphValues::PoseKey(const lc::Time timestamp) const { return GetKey(&sym::P, timestamp); } +boost::optional CombinedNavStateGraphValues::PoseKey(const lc::Time timestamp) const { + return GetKey(&sym::P, timestamp); +} -boost::optional GraphValues::GetCombinedNavState(const lc::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::GetCombinedNavState(const lc::Time timestamp) const { if (!HasKey(timestamp)) { LogError("GetCombinedNavState: No CombinedNavState found at timestamp."); return boost::none; @@ -179,53 +188,21 @@ boost::optional GraphValues::GetCombinedNavState(const lc: return lc::CombinedNavState{*pose, *velocity, *bias, timestamp}; } -double GraphValues::Duration() const { +double CombinedNavStateGraphValues::Duration() const { if (Empty()) return 0; return (*LatestTimestamp() - *OldestTimestamp()); } -int GraphValues::NumStates() const { return timestamp_key_index_map_.size(); } +int CombinedNavStateGraphValues::NumStates() const { return timestamp_key_index_map_.size(); } -boost::optional GraphValues::Timestamp(const int key_index) const { +boost::optional CombinedNavStateGraphValues::Timestamp(const int key_index) const { for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { if (timestamp_key_index_pair.second == key_index) return timestamp_key_index_pair.first; } return boost::none; } -bool GraphValues::HasFeature(const lm::FeatureId id) const { return (feature_id_key_map_.count(id) > 0); } - -boost::optional GraphValues::FeatureKey(const lm::FeatureId id) const { - if (!HasFeature(id)) return boost::none; - return feature_id_key_map_.at(id); -} - -gtsam::Key GraphValues::CreateFeatureKey() const { return sym::F(++feature_key_index_); } - -gtsam::KeyVector GraphValues::FeatureKeys() const { - gtsam::KeyVector feature_keys; - for (const auto& feature_id_key_pair : feature_id_key_map_) { - feature_keys.emplace_back(feature_id_key_pair.second); - } - return feature_keys; -} - -bool GraphValues::AddFeature(const lm::FeatureId id, const gtsam::Point3& feature_point, const gtsam::Key& key) { - if (HasFeature(id)) { - LogError("AddFeature: Feature already exists."); - return false; - } - - if (values_.exists(key)) { - LogError("AddFeature: Key already exists in values."); - } - - feature_id_key_map_.emplace(id, key); - values_.insert(key, feature_point); - return true; -} - -boost::optional GraphValues::LatestCombinedNavStateKeyIndex() const { +boost::optional CombinedNavStateGraphValues::LatestCombinedNavStateKeyIndex() const { if (Empty()) { LogError("LatestCombinedNavStateKeyIndex: No combined nav states available."); return boost::none; @@ -233,9 +210,7 @@ boost::optional GraphValues::LatestCombinedNavStateKeyIndex() const { return timestamp_key_index_map_.crbegin()->second; } -int GraphValues::NumFeatures() const { return feature_id_key_map_.size(); } - -boost::optional GraphValues::OldestCombinedNavStateKeyIndex() const { +boost::optional CombinedNavStateGraphValues::OldestCombinedNavStateKeyIndex() const { if (Empty()) { LogError("OldestCombinedNavStateKeyIndex: No combined nav states available."); return boost::none; @@ -243,7 +218,7 @@ boost::optional GraphValues::OldestCombinedNavStateKeyIndex() const { return timestamp_key_index_map_.cbegin()->second; } -boost::optional> GraphValues::LatestBias() const { +boost::optional> CombinedNavStateGraphValues::LatestBias() const { if (Empty()) { LogError("LatestBias: No bias values available."); return boost::none; @@ -252,7 +227,7 @@ boost::optional> GraphValues:: const lc::Time timestamp = timestamp_key_index_map_.crbegin()->first; const int key_index = timestamp_key_index_map_.crbegin()->second; - if (!values_.exists(sym::B(key_index))) { + if (!values().exists(sym::B(key_index))) { LogError("LatestBias: Bias key not present in values."); return boost::none; } @@ -266,7 +241,7 @@ boost::optional> GraphValues:: return std::pair{*bias, timestamp}; } -boost::optional GraphValues::LowerBoundOrEqualTimestamp(const lc::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::LowerBoundOrEqualTimestamp(const lc::Time timestamp) const { const auto lower_and_upper_bound_timestamp = LowerAndUpperBoundTimestamp(timestamp); if (!lower_and_upper_bound_timestamp.first && !lower_and_upper_bound_timestamp.second) { LogError("LowerBoundOrEqualTimestamp: Failed to get lower or upper bound timestamps."); @@ -281,7 +256,8 @@ boost::optional GraphValues::LowerBoundOrEqualTimestamp(const lc::Time return lower_and_upper_bound_timestamp.first; } -boost::optional GraphValues::LowerBoundOrEqualCombinedNavState(const lc::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::LowerBoundOrEqualCombinedNavState( + const lc::Time timestamp) const { const auto lower_bound_or_equal_timestamp = LowerBoundOrEqualTimestamp(timestamp); if (!lower_bound_or_equal_timestamp) { LogDebug("LowerBoundOrEqualCombinedNavState: Failed to get lower bound or equal timestamp."); @@ -291,13 +267,13 @@ boost::optional GraphValues::LowerBoundOrEqualCombinedNavS return GetCombinedNavState(*lower_bound_or_equal_timestamp); } -boost::optional GraphValues::SlideWindowNewOldestTime() const { +boost::optional CombinedNavStateGraphValues::SlideWindowNewOldestTime() const { if (Empty()) { LogDebug("SlideWindowOldestTime: No states in map."); return boost::none; } - if (NumStates() <= params_.min_num_states) { + if (NumStates() <= params().min_num_states) { LogDebug("SlideWindowOldestTime: Not enough states to remove."); return boost::none; } @@ -306,7 +282,7 @@ boost::optional GraphValues::SlideWindowNewOldestTime() const { LogDebug("SlideWindowOldestTime: Starting total num states: " << timestamp_key_index_map_.size()); LogDebug("SlideWindowOldestTime: Starting total duration is " << total_duration); const lc::Time ideal_oldest_allowed_state = - std::max(0.0, timestamp_key_index_map_.crbegin()->first - params_.ideal_duration); + std::max(0.0, timestamp_key_index_map_.crbegin()->first - params().ideal_duration); int num_states_to_be_removed = 0; // Ensures that new oldest time is consistent with a number of states <= max_num_states @@ -315,9 +291,9 @@ boost::optional GraphValues::SlideWindowNewOldestTime() const { for (const auto& timestamp_key_pair : timestamp_key_index_map_) { ++num_states_to_be_removed; const int new_num_states = NumStates() - num_states_to_be_removed; - if (new_num_states > params_.max_num_states) continue; + if (new_num_states > params().max_num_states) continue; const auto& time = timestamp_key_pair.first; - if (new_num_states <= params_.min_num_states) return time; + if (new_num_states <= params().min_num_states) return time; if (time >= ideal_oldest_allowed_state) return time; } @@ -326,7 +302,8 @@ boost::optional GraphValues::SlideWindowNewOldestTime() const { } // Add timestamp and keys to timestamp_key_index_map, and values to values -bool GraphValues::AddCombinedNavState(const lc::CombinedNavState& combined_nav_state, const int key_index) { +bool CombinedNavStateGraphValues::AddCombinedNavState(const lc::CombinedNavState& combined_nav_state, + const int key_index) { if (HasKey(combined_nav_state.timestamp())) { LogError( "AddCombinedNavState: Timestamp key index map already " @@ -334,29 +311,29 @@ bool GraphValues::AddCombinedNavState(const lc::CombinedNavState& combined_nav_s return false; } timestamp_key_index_map_.emplace(combined_nav_state.timestamp(), key_index); - if (values_.exists(sym::P(key_index))) { + if (values().exists(sym::P(key_index))) { LogError("AddCombinedNavState: Pose key already in values."); return false; } - if (values_.exists(sym::V(key_index))) { + if (values().exists(sym::V(key_index))) { LogError("AddCombinedNavState: Velocity key already in values."); return false; } - if (values_.exists(sym::B(key_index))) { + if (values().exists(sym::B(key_index))) { LogError("AddCombinedNavState: Bias key already in values."); return false; } - values_.insert(sym::P(key_index), combined_nav_state.pose()); - values_.insert(sym::V(key_index), combined_nav_state.velocity()); - values_.insert(sym::B(key_index), combined_nav_state.bias()); + values().insert(sym::P(key_index), combined_nav_state.pose()); + values().insert(sym::V(key_index), combined_nav_state.velocity()); + values().insert(sym::B(key_index), combined_nav_state.bias()); LogDebug("AddCombinedNavState: Added key_index " << key_index); LogDebug("AddCombinedNavState: Added timestamp " << std::setprecision(15) << combined_nav_state.timestamp()); return true; } -boost::optional GraphValues::KeyIndex(const lc::Time timestamp) const { +boost::optional CombinedNavStateGraphValues::KeyIndex(const lc::Time timestamp) const { if (!HasKey(timestamp)) { LogError("KeyIndex: No key found for timestamp."); return boost::none; @@ -365,33 +342,7 @@ boost::optional GraphValues::KeyIndex(const lc::Time timestamp) const { return timestamp_key_index_map_.at(timestamp); } -void GraphValues::UpdateValues(const gtsam::Values& new_values) { values_ = new_values; } - -gtsam::NonlinearFactorGraph GraphValues::RemoveOldFactors(const gtsam::KeyVector& old_keys, - gtsam::NonlinearFactorGraph& graph) { - gtsam::NonlinearFactorGraph removed_factors; - if (old_keys.empty()) return removed_factors; - - for (auto factor_it = graph.begin(); factor_it != graph.end();) { - bool found_key = false; - for (const auto& key : old_keys) { - if ((*factor_it)->find(key) != (*factor_it)->end()) { - found_key = true; - break; - } - } - if (found_key) { - removed_factors.push_back(*factor_it); - factor_it = graph.erase(factor_it); - } else { - ++factor_it; - } - } - - return removed_factors; -} - -int GraphValues::RemoveOldCombinedNavStates(const lc::Time oldest_allowed_time) { +int CombinedNavStateGraphValues::RemoveOldCombinedNavStates(const lc::Time oldest_allowed_time) { int num_states_removed = 0; while (timestamp_key_index_map_.begin()->first < oldest_allowed_time) { RemoveCombinedNavState(timestamp_key_index_map_.begin()->first); @@ -405,7 +356,8 @@ int GraphValues::RemoveOldCombinedNavStates(const lc::Time oldest_allowed_time) return num_states_removed; } -gtsam::KeyVector GraphValues::OldKeys(const lc::Time oldest_allowed_time) const { +gtsam::KeyVector CombinedNavStateGraphValues::OldKeys(const lc::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const { gtsam::KeyVector old_keys; for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { if (timestamp_key_index_pair.first >= oldest_allowed_time) break; @@ -420,7 +372,7 @@ gtsam::KeyVector GraphValues::OldKeys(const lc::Time oldest_allowed_time) const // Removes keys from timestamp_key_index_map, values from values // Assumes for each stamped_key_index there is a Pose, Velocity, and Bias key -bool GraphValues::RemoveCombinedNavState(const lc::Time timestamp) { +bool CombinedNavStateGraphValues::RemoveCombinedNavState(const lc::Time timestamp) { if (!HasKey(timestamp)) { LogError( "RemoveCombinedNavState: Timestamp not found in timestamp " @@ -432,20 +384,20 @@ bool GraphValues::RemoveCombinedNavState(const lc::Time timestamp) { bool removed_values = true; // Remove key/value pairs from values - if (values_.exists(sym::P(key_index))) { - values_.erase(sym::P(key_index)); + if (values().exists(sym::P(key_index))) { + values().erase(sym::P(key_index)); } else { LogError("RemoveCombinedNavState: Pose key not present in values."); removed_values = false; } - if (values_.exists(sym::V(key_index))) { - values_.erase(sym::V(key_index)); + if (values().exists(sym::V(key_index))) { + values().erase(sym::V(key_index)); } else { LogError("RemoveCombinedNavState: Velocity key not present in values."); removed_values = false; } - if (values_.exists(sym::B(key_index))) { - values_.erase(sym::B(key_index)); + if (values().exists(sym::B(key_index))) { + values().erase(sym::B(key_index)); } else { LogError("RemoveCombinedNavState: Bias key not present in values."); removed_values = false; @@ -455,40 +407,4 @@ bool GraphValues::RemoveCombinedNavState(const lc::Time timestamp) { LogDebug("RemoveCombinedNavState: Removed timestamp" << std::setprecision(15) << timestamp); return removed_values; } - -gtsam::KeyVector GraphValues::OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const { - using ProjectionFactor = gtsam::GenericProjectionFactor; - gtsam::KeyVector old_features; - for (const auto& feature_id_key_pair : feature_id_key_map_) { - const auto& key = feature_id_key_pair.second; - int num_factors = 0; - for (const auto& factor : factors) { - // Only consider projection factors for min num feature factors - const auto projection_factor = dynamic_cast(factor.get()); - if (!projection_factor) continue; - if (factor->find(key) != factor->end()) { - ++num_factors; - if (num_factors >= params_.min_num_factors_per_feature) break; - } - } - - if (num_factors < params_.min_num_factors_per_feature) { - old_features.emplace_back(key); - } - } - return old_features; -} - -void GraphValues::RemoveOldFeatures(const gtsam::KeyVector& old_keys) { - for (const auto& key : old_keys) { - values_.erase(key); - for (auto feature_id_key_it = feature_id_key_map_.begin(); feature_id_key_it != feature_id_key_map_.end();) { - if (feature_id_key_it->second == key) { - feature_id_key_it = feature_id_key_map_.erase(feature_id_key_it); - } else { - ++feature_id_key_it; - } - } - } -} } // namespace graph_localizer diff --git a/localization/graph_localizer/src/combined_nav_state_node_updater.cc b/localization/graph_localizer/src/combined_nav_state_node_updater.cc new file mode 100644 index 0000000000..981f8304f8 --- /dev/null +++ b/localization/graph_localizer/src/combined_nav_state_node_updater.cc @@ -0,0 +1,402 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace ii = imu_integration; +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; +CombinedNavStateNodeUpdater::CombinedNavStateNodeUpdater( + const CombinedNavStateNodeUpdaterParams& params, + std::shared_ptr latest_imu_integrator, std::shared_ptr values) + : params_(params), + latest_imu_integrator_(std::move(latest_imu_integrator)), + graph_values_(new CombinedNavStateGraphValues(params.graph_values, std::move(values))), + key_index_(0) { + const gtsam::Vector6 pose_prior_noise_sigmas( + (gtsam::Vector(6) << params_.starting_prior_translation_stddev, params_.starting_prior_translation_stddev, + params_.starting_prior_translation_stddev, params_.starting_prior_quaternion_stddev, + params_.starting_prior_quaternion_stddev, params_.starting_prior_quaternion_stddev) + .finished()); + const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params_.starting_prior_velocity_stddev, + params_.starting_prior_velocity_stddev, + params_.starting_prior_velocity_stddev) + .finished()); + const gtsam::Vector6 bias_prior_noise_sigmas( + (gtsam::Vector(6) << params_.starting_prior_accel_bias_stddev, params_.starting_prior_accel_bias_stddev, + params_.starting_prior_accel_bias_stddev, params_.starting_prior_gyro_bias_stddev, + params_.starting_prior_gyro_bias_stddev, params_.starting_prior_gyro_bias_stddev) + .finished()); + global_N_body_start_noise_.pose_noise = Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_prior_noise_sigmas)), params_.huber_k); + global_N_body_start_noise_.velocity_noise = + Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), + params_.huber_k); + global_N_body_start_noise_.bias_noise = Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(bias_prior_noise_sigmas)), params_.huber_k); +} + +void CombinedNavStateNodeUpdater::AddInitialValuesAndPriors(gtsam::NonlinearFactorGraph& factors) { + AddInitialValuesAndPriors(params_.global_N_body_start, global_N_body_start_noise_, factors); +} + +void CombinedNavStateNodeUpdater::AddInitialValuesAndPriors(const lc::CombinedNavState& global_N_body, + const lc::CombinedNavStateNoise& noise, + gtsam::NonlinearFactorGraph& factors) { + const int key_index = GenerateKeyIndex(); + graph_values_->AddCombinedNavState(global_N_body, key_index); + AddPriors(global_N_body, noise, factors); +} + +void CombinedNavStateNodeUpdater::AddPriors(const lc::CombinedNavState& global_N_body, + const lc::CombinedNavStateNoise& noise, + gtsam::NonlinearFactorGraph& factors) { + const auto key_index = graph_values_->KeyIndex(global_N_body.timestamp()); + if (!key_index) { + LogError("AddPriors: Failed to get key index."); + return; + } + // TODO(rsoussan): Ensure symbols not used by other node updaters + gtsam::PriorFactor pose_prior_factor(sym::P(*key_index), global_N_body.pose(), noise.pose_noise); + factors.push_back(pose_prior_factor); + gtsam::PriorFactor velocity_prior_factor(sym::V(*key_index), global_N_body.velocity(), + noise.velocity_noise); + factors.push_back(velocity_prior_factor); + gtsam::PriorFactor bias_prior_factor(sym::B(*key_index), global_N_body.bias(), + noise.bias_noise); + factors.push_back(bias_prior_factor); +} + +bool CombinedNavStateNodeUpdater::SlideWindow(const lc::Time oldest_allowed_timestamp, + const boost::optional& marginals, + const gtsam::KeyVector& old_keys, const double huber_k, + gtsam::NonlinearFactorGraph& factors) { + graph_values_->RemoveOldCombinedNavStates(oldest_allowed_timestamp); + if (params_.add_priors) { + // Add prior to oldest nav state using covariances from last round of + // optimization + const auto global_N_body_oldest = graph_values_->OldestCombinedNavState(); + if (!global_N_body_oldest) { + LogError("SlideWindow: Failed to get oldest combined nav state."); + return false; + } + + LogDebug("SlideWindow: Oldest state time: " << global_N_body_oldest->timestamp()); + + const auto key_index = graph_values_->OldestCombinedNavStateKeyIndex(); + if (!key_index) { + LogError("SlideWindow: Failed to get oldest combined nav state key index."); + return false; + } + + LogDebug("SlideWindow: key index: " << *key_index); + + // Make sure priors are removed before adding new ones + RemovePriors(*key_index, factors); + if (marginals) { + lc::CombinedNavStateNoise noise; + noise.pose_noise = + Robust(gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::P(*key_index))), huber_k); + noise.velocity_noise = + Robust(gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::V(*key_index))), huber_k); + auto bias_covariance = marginals->marginalCovariance(sym::B(*key_index)); + if (params_.threshold_bias_uncertainty) ThresholdBiasUncertainty(bias_covariance); + noise.bias_noise = Robust(gtsam::noiseModel::Gaussian::Covariance(bias_covariance), huber_k); + AddPriors(*global_N_body_oldest, noise, factors); + } else { + // TODO(rsoussan): Add seperate marginal fallback sigmas instead of relying on starting prior sigmas + AddPriors(*global_N_body_oldest, global_N_body_start_noise_, factors); + } + } + + return true; +} + +void CombinedNavStateNodeUpdater::ThresholdBiasUncertainty(gtsam::Matrix& bias_covariance) const { + // Only checking sigmas for now + const auto bias_covariance_sigmas = bias_covariance.diagonal().cwiseSqrt(); + bool valid_sigmas = true; + for (int i = 0; i < 3; ++i) { + if (bias_covariance_sigmas[i] > params_.accel_bias_stddev_threshold) { + valid_sigmas = false; + break; + } + } + if (valid_sigmas) { + for (int i = 3; i < 6; ++i) { + if (bias_covariance_sigmas[i] > params_.gyro_bias_stddev_threshold) { + valid_sigmas = false; + break; + } + } + } + if (valid_sigmas) return; + gtsam::Vector6 new_sigmas; + for (int i = 0; i < 3; ++i) { + new_sigmas[i] = std::min(bias_covariance_sigmas[i], params_.accel_bias_stddev_threshold); + } + for (int i = 3; i < 6; ++i) { + new_sigmas[i] = std::min(bias_covariance_sigmas[i], params_.gyro_bias_stddev_threshold); + } + const gtsam::Vector6 new_variances = new_sigmas.cwiseAbs2(); + bias_covariance = new_variances.asDiagonal(); +} + +go::NodeUpdaterType CombinedNavStateNodeUpdater::type() const { return go::NodeUpdaterType::CombinedNavState; } + +boost::optional CombinedNavStateNodeUpdater::SlideWindowNewOldestTime() const { + return graph_values_->SlideWindowNewOldestTime(); +} + +gtsam::KeyVector CombinedNavStateNodeUpdater::OldKeys(const lc::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const { + return graph_values_->OldKeys(oldest_allowed_time, graph); +} + +boost::optional CombinedNavStateNodeUpdater::GetKey(go::KeyCreatorFunction key_creator_function, + const lc::Time timestamp) const { + return graph_values_->GetKey(key_creator_function, timestamp); +} + +boost::optional CombinedNavStateNodeUpdater::OldestTimestamp() const { + return graph_values_->OldestTimestamp(); +} + +boost::optional CombinedNavStateNodeUpdater::LatestTimestamp() const { + return graph_values_->LatestTimestamp(); +} + +std::shared_ptr CombinedNavStateNodeUpdater::shared_graph_values() const { + return graph_values_; +} + +std::shared_ptr CombinedNavStateNodeUpdater::shared_graph_values() { + return graph_values_; +} + +const CombinedNavStateGraphValues& CombinedNavStateNodeUpdater::graph_values() const { return *graph_values_; } + +void CombinedNavStateNodeUpdater::RemovePriors(const int key_index, gtsam::NonlinearFactorGraph& factors) { + int removed_factors = 0; + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + bool erase_factor = false; + const auto pose_prior_factor = dynamic_cast*>(factor_it->get()); + const auto loc_pose_factor = dynamic_cast(factor_it->get()); + if (pose_prior_factor && !loc_pose_factor && pose_prior_factor->key() == sym::P(key_index)) { + erase_factor = true; + } + const auto velocity_prior_factor = dynamic_cast*>(factor_it->get()); + if (velocity_prior_factor && velocity_prior_factor->key() == sym::V(key_index)) { + erase_factor = true; + } + const auto bias_prior_factor = dynamic_cast*>(factor_it->get()); + if (bias_prior_factor && bias_prior_factor->key() == sym::B(key_index)) { + erase_factor = true; + } + + if (erase_factor) { + factor_it = factors.erase(factor_it); + ++removed_factors; + } else { + ++factor_it; + continue; + } + } + LogDebug("RemovePriors: Erase " << removed_factors << " factors."); +} + +int CombinedNavStateNodeUpdater::GenerateKeyIndex() { return key_index_++; } + +bool CombinedNavStateNodeUpdater::Update(const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) { + return AddOrSplitImuFactorIfNeeded(timestamp, factors, *graph_values_); +} + +bool CombinedNavStateNodeUpdater::AddOrSplitImuFactorIfNeeded(const lc::Time timestamp, + gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values) { + if (graph_values.HasKey(timestamp)) { + LogDebug( + "AddOrSplitImuFactorIfNeeded: CombinedNavState exists at " + "timestamp, nothing to do."); + return true; + } + + const auto latest_timestamp = graph_values.LatestTimestamp(); + if (!latest_timestamp) { + LogError("AddOrSplitImuFactorIfNeeded: Failed to get latest timestamp."); + return false; + } + + if (timestamp > *latest_timestamp) { + LogDebug( + "AddOrSplitImuFactorIfNeeded: Creating and adding latest imu " + "factor and nav state."); + return CreateAndAddLatestImuFactorAndCombinedNavState(timestamp, factors, graph_values); + } else { + LogDebug("AddOrSplitImuFactorIfNeeded: Splitting old imu factor."); + return SplitOldImuFactorAndAddCombinedNavState(timestamp, factors, graph_values); + } +} + +bool CombinedNavStateNodeUpdater::CreateAndAddLatestImuFactorAndCombinedNavState( + const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors, CombinedNavStateGraphValues& graph_values) { + if (!latest_imu_integrator_->IntegrateLatestImuMeasurements(timestamp)) { + LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to integrate latest imu measurements."); + return false; + } + + const auto latest_combined_nav_state = graph_values.LatestCombinedNavState(); + if (!latest_combined_nav_state) { + LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest combined nav state."); + return false; + } + if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*latest_combined_nav_state, latest_imu_integrator_->pim(), + factors, graph_values)) { + LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to create and add imu factor."); + return false; + } + + const auto latest_bias = graph_values.LatestBias(); + if (!latest_bias) { + LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest bias."); + return false; + } + + latest_imu_integrator_->ResetPimIntegrationAndSetBias(latest_bias->first); + return true; +} + +bool CombinedNavStateNodeUpdater::CreateAndAddImuFactorAndPredictedCombinedNavState( + const lc::CombinedNavState& global_N_body, const gtsam::PreintegratedCombinedMeasurements& pim, + gtsam::NonlinearFactorGraph& factors, CombinedNavStateGraphValues& graph_values) { + const auto key_index_0 = graph_values.KeyIndex(global_N_body.timestamp()); + if (!key_index_0) { + LogError("CreateAndAddImuFactorAndPredictedCombinedNavState: Failed to get first key index."); + return false; + } + + const lc::CombinedNavState global_N_body_predicted = ii::PimPredict(global_N_body, pim); + const int key_index_1 = GenerateKeyIndex(); + const auto combined_imu_factor = ii::MakeCombinedImuFactor(*key_index_0, key_index_1, pim); + factors.push_back(combined_imu_factor); + graph_values.AddCombinedNavState(global_N_body_predicted, key_index_1); + return true; +} + +bool CombinedNavStateNodeUpdater::SplitOldImuFactorAndAddCombinedNavState(const lc::Time timestamp, + gtsam::NonlinearFactorGraph& factors, + CombinedNavStateGraphValues& graph_values) { + const auto timestamp_bounds = graph_values.LowerAndUpperBoundTimestamp(timestamp); + if (!timestamp_bounds.first || !timestamp_bounds.second) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get upper and lower bound timestamp."); + return false; + } + + const lc::Time lower_bound_time = *(timestamp_bounds.first); + const lc::Time upper_bound_time = *(timestamp_bounds.second); + + if (timestamp < lower_bound_time || timestamp > upper_bound_time) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Timestamp is not within bounds of existing timestamps."); + return false; + } + + const auto lower_bound_key_index = graph_values.KeyIndex(lower_bound_time); + const auto upper_bound_key_index = graph_values.KeyIndex(upper_bound_time); + if (!lower_bound_key_index || !upper_bound_key_index) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower and upper bound key indices."); + return false; + } + + // get old imu factor, delete it + bool removed_old_imu_factor = false; + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + if (dynamic_cast(factor_it->get()) && + graph_values.ContainsCombinedNavStateKey(**factor_it, *lower_bound_key_index) && + graph_values.ContainsCombinedNavStateKey(**factor_it, *upper_bound_key_index)) { + factors.erase(factor_it); + removed_old_imu_factor = true; + break; + } + ++factor_it; + } + if (!removed_old_imu_factor) { + LogError( + "SplitOldImuFactorAndAddCombinedNavState: Failed to remove " + "old imu factor."); + return false; + } + + const auto lower_bound_bias = graph_values.at(sym::B(*lower_bound_key_index)); + if (!lower_bound_bias) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound bias."); + return false; + } + + // Add first factor and new nav state at timestamp + auto first_integrated_pim = latest_imu_integrator_->IntegratedPim(*lower_bound_bias, lower_bound_time, timestamp, + latest_imu_integrator_->pim_params()); + if (!first_integrated_pim) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create first integrated pim."); + return false; + } + + const auto lower_bound_combined_nav_state = graph_values.GetCombinedNavState(lower_bound_time); + if (!lower_bound_combined_nav_state) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound combined nav state."); + return false; + } + + if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*lower_bound_combined_nav_state, *first_integrated_pim, + factors, graph_values)) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create and add imu factor."); + return false; + } + + // Add second factor, use lower_bound_bias as starting bias since that is the + // best estimate available + auto second_integrated_pim = latest_imu_integrator_->IntegratedPim(*lower_bound_bias, timestamp, upper_bound_time, + latest_imu_integrator_->pim_params()); + if (!second_integrated_pim) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create second integrated pim."); + return false; + } + + // New nav state already added so just get its key index + const auto new_key_index = graph_values.KeyIndex(timestamp); + if (!new_key_index) { + LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get new key index."); + return false; + } + + const auto combined_imu_factor = + ii::MakeCombinedImuFactor(*new_key_index, *upper_bound_key_index, *second_integrated_pim); + factors.push_back(combined_imu_factor); + return true; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_point_graph_values.cc b/localization/graph_localizer/src/feature_point_graph_values.cc new file mode 100644 index 0000000000..471272b520 --- /dev/null +++ b/localization/graph_localizer/src/feature_point_graph_values.cc @@ -0,0 +1,121 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include +#include +#include + +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace lm = localization_measurements; +FeaturePointGraphValues::FeaturePointGraphValues(std::shared_ptr values) + : go::GraphValues(std::move(values)), feature_key_index_(0) {} + +boost::optional FeaturePointGraphValues::GetKey(go::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const { + return boost::none; +} + +boost::optional FeaturePointGraphValues::OldestTimestamp() const { return boost::none; } + +boost::optional FeaturePointGraphValues::LatestTimestamp() const { return boost::none; } + +boost::optional FeaturePointGraphValues::SlideWindowNewOldestTime() const { return boost::none; } + +bool FeaturePointGraphValues::HasFeature(const lm::FeatureId id) const { return (feature_id_key_map_.count(id) > 0); } + +boost::optional FeaturePointGraphValues::FeatureKey(const lm::FeatureId id) const { + if (!HasFeature(id)) return boost::none; + return feature_id_key_map_.at(id); +} + +gtsam::Key FeaturePointGraphValues::CreateFeatureKey() const { return sym::F(++feature_key_index_); } + +gtsam::KeyVector FeaturePointGraphValues::FeatureKeys() const { + gtsam::KeyVector feature_keys; + for (const auto& feature_id_key_pair : feature_id_key_map_) { + feature_keys.emplace_back(feature_id_key_pair.second); + } + return feature_keys; +} + +bool FeaturePointGraphValues::AddFeature(const lm::FeatureId id, const gtsam::Point3& feature_point, + const gtsam::Key& key) { + if (HasFeature(id)) { + LogError("AddFeature: Feature already exists."); + return false; + } + + if (values().exists(key)) { + LogError("AddFeature: Key already exists in values."); + } + + feature_id_key_map_.emplace(id, key); + values().insert(key, feature_point); + return true; +} + +int FeaturePointGraphValues::NumFeatures() const { return feature_id_key_map_.size(); } + +gtsam::KeyVector FeaturePointGraphValues::OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& factors) const { + using ProjectionFactor = gtsam::GenericProjectionFactor; + gtsam::KeyVector old_features; + for (const auto& feature_id_key_pair : feature_id_key_map_) { + const auto& key = feature_id_key_pair.second; + int num_factors = 0; + for (const auto& factor : factors) { + // Only consider projection factors for min num feature factors + const auto projection_factor = dynamic_cast(factor.get()); + if (!projection_factor) continue; + if (factor->find(key) != factor->end()) { + ++num_factors; + // if (num_factors >= params_.min_num_factors_per_feature) break; + } + } + + // if (num_factors < params_.min_num_factors_per_feature) { + if (num_factors <= 0) { + old_features.emplace_back(key); + } + } + return old_features; +} + +void FeaturePointGraphValues::RemoveOldFeatures(const gtsam::KeyVector& old_keys) { + for (const auto& key : old_keys) { + // TODO(rsoussan): test this + if (gtsam::Symbol(key).chr() != 'F') continue; + values().erase(key); + for (auto feature_id_key_it = feature_id_key_map_.begin(); feature_id_key_it != feature_id_key_map_.end();) { + if (feature_id_key_it->second == key) { + feature_id_key_it = feature_id_key_map_.erase(feature_id_key_it); + } else { + ++feature_id_key_it; + } + } + } +} + +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_point_node_updater.cc b/localization/graph_localizer/src/feature_point_node_updater.cc new file mode 100644 index 0000000000..3bb355776b --- /dev/null +++ b/localization/graph_localizer/src/feature_point_node_updater.cc @@ -0,0 +1,114 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; + +// TODO(rsoussan): Make new node updater base class that doesn't have functions left empty here? +FeaturePointNodeUpdater::FeaturePointNodeUpdater(const FeaturePointNodeUpdaterParams& params, + std::shared_ptr values) + : params_(params), feature_point_graph_values_(new FeaturePointGraphValues(std::move(values))) {} + +void FeaturePointNodeUpdater::AddInitialValuesAndPriors(const lc::FeaturePoint3d& global_t_point, + const lc::FeaturePoint3dNoise& noise, + gtsam::NonlinearFactorGraph& factors) {} + +void FeaturePointNodeUpdater::AddPriors(const lc::FeaturePoint3d& global_t_point, const lc::FeaturePoint3dNoise& noise, + gtsam::NonlinearFactorGraph& factors) {} + +bool FeaturePointNodeUpdater::SlideWindow(const lc::Time oldest_allowed_timestamp, + const boost::optional& marginals, + const gtsam::KeyVector& old_keys, const double huber_k, + gtsam::NonlinearFactorGraph& factors) { + feature_point_graph_values_->RemoveOldFeatures(old_keys); + if (marginals) UpdatePointPriors(*marginals, factors); + return true; +} + +void FeaturePointNodeUpdater::UpdatePointPriors(const gtsam::Marginals& marginals, + gtsam::NonlinearFactorGraph& factors) { + const auto feature_keys = feature_point_graph_values_->FeatureKeys(); + for (const auto& feature_key : feature_keys) { + const auto world_t_point = feature_point_graph_values_->at(feature_key); + if (!world_t_point) { + LogError("UpdatePointPriors: Failed to get world_t_point."); + continue; + } + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + const auto point_prior_factor = dynamic_cast*>(factor_it->get()); + if (point_prior_factor && (point_prior_factor->key() == feature_key)) { + // Erase old prior + factor_it = factors.erase(factor_it); + // Add updated one + const auto point_prior_noise = + Robust(gtsam::noiseModel::Gaussian::Covariance(marginals.marginalCovariance(feature_key)), params_.huber_k); + const gtsam::PriorFactor point_prior_factor(feature_key, *world_t_point, point_prior_noise); + factors.push_back(point_prior_factor); + // Only one point prior per feature + break; + } else { + ++factor_it; + } + } + } +} + +go::NodeUpdaterType FeaturePointNodeUpdater::type() const { return go::NodeUpdaterType::FeaturePoint; } + +bool FeaturePointNodeUpdater::Update(const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) { return true; } + +boost::optional FeaturePointNodeUpdater::SlideWindowNewOldestTime() const { + return feature_point_graph_values_->SlideWindowNewOldestTime(); +} + +gtsam::KeyVector FeaturePointNodeUpdater::OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const { + return feature_point_graph_values_->OldKeys(oldest_allowed_time, graph); +} + +boost::optional FeaturePointNodeUpdater::GetKey(go::KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const { + return feature_point_graph_values_->GetKey(key_creator_function, timestamp); +} + +boost::optional FeaturePointNodeUpdater::OldestTimestamp() const { + return feature_point_graph_values_->OldestTimestamp(); +} + +boost::optional FeaturePointNodeUpdater::LatestTimestamp() const { + return feature_point_graph_values_->LatestTimestamp(); +} + +int FeaturePointNodeUpdater::NumFeatures() const { return feature_point_graph_values_->NumFeatures(); } + +std::shared_ptr FeaturePointNodeUpdater::shared_feature_point_graph_values() { + return feature_point_graph_values_; +} + +const FeaturePointGraphValues& FeaturePointNodeUpdater::feature_point_graph_values() const { + return *feature_point_graph_values_; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_track.cc b/localization/graph_localizer/src/feature_track.cc new file mode 100644 index 0000000000..8c7d92dd1c --- /dev/null +++ b/localization/graph_localizer/src/feature_track.cc @@ -0,0 +1,128 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +namespace graph_localizer { +namespace lc = localization_common; +namespace lm = localization_measurements; +FeatureTrack::FeatureTrack(const localization_measurements::FeatureId id) : id_(id) {} + +void FeatureTrack::AddMeasurement(const lc::Time timestamp, const lm::FeaturePoint& feature_point) { + points_.emplace(timestamp, feature_point); +} + +void FeatureTrack::RemoveOldMeasurements(const lc::Time oldest_allowed_timestamp) { + points_.erase(points_.begin(), points_.lower_bound(oldest_allowed_timestamp)); +} + +bool FeatureTrack::HasMeasurement(const lc::Time timestamp) { return (points_.count(timestamp) > 0); } + +const FeatureTrack::Points& FeatureTrack::points() const { return points_; } + +const localization_measurements::FeatureId& FeatureTrack::id() const { return id_; } + +size_t FeatureTrack::size() const { return points_.size(); } + +bool FeatureTrack::empty() const { return points_.empty(); } + +std::vector FeatureTrack::AllowedPoints(const std::set& allowed_timestamps) const { + std::vector allowed_points; + // Start with oldest points + for (auto point_it = points_.begin(); point_it != points_.end(); ++point_it) { + if (allowed_timestamps.count(point_it->second.timestamp) <= 0) continue; + allowed_points.emplace_back(point_it->second); + } + return allowed_points; +} + +std::vector FeatureTrack::LatestPointsInWindow(const double duration) const { + std::vector latest_points; + const auto latest_timestamp = LatestTimestamp(); + if (!latest_timestamp) return {}; + const lc::Time oldest_allowed_time = *latest_timestamp - duration; + // Start with latest points + for (auto point_it = points_.rbegin(); point_it != points_.rend(); ++point_it) { + if (point_it->second.timestamp < oldest_allowed_time) break; + latest_points.push_back(point_it->second); + } + return latest_points; +} + +std::vector FeatureTrack::LatestPoints(const int spacing) const { + std::vector latest_points; + int i = 0; + // Start with latest points + for (auto point_it = points_.rbegin(); point_it != points_.rend(); ++point_it) { + if (i++ % (spacing + 1) != 0) continue; + latest_points.push_back(point_it->second); + } + return latest_points; +} + +bool FeatureTrack::SpacingFits(const int spacing, const int max_num_points) const { + // Since we include the latest point, the points included for spacing and max_num_points + // is 1 for the latest point plus a point at intervals of spacing + 1 for max_num_points - 1 (excludes latest point). + // 1 0 0 1 0 0 1 0 0 -> here a 1 indicates a point used, the first 1 is the latest point, and the spacing is 2. + // In this case if max_num_points <= 3 this suceeds and otherwise this fails as fewer than 3 points would be included + // with the desired spacing. + return ((spacing + 1) * (max_num_points - 1) + 1) <= static_cast(size()); +} + +int FeatureTrack::MaxSpacing(const int max_num_points) const { + // Avoid divide by zero and other corner cases + if (max_num_points <= 1 || static_cast(size()) <= 0) return 0; + // Derived using equation from SpacingFits + // (spacing + 1 ) * (max_num_points - 1) + 1 = size + // -> spacing = (size - max_num_points)/(max_num_points - 1) + return std::max(0, (static_cast(size()) - max_num_points) / (max_num_points - 1)); +} + +int FeatureTrack::ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const { + // Check Ideal Case + if (SpacingFits(ideal_spacing, ideal_max_num_points)) return ideal_spacing; + // Check too few points case + if (static_cast(size()) <= ideal_max_num_points) return 0; + // Derive new optimal spacing for ideal_max_num_points + for (int spacing = ideal_spacing - 1; spacing >= 0; --spacing) { + if (SpacingFits(spacing, ideal_max_num_points)) return spacing; + } + return 0; +} + +boost::optional FeatureTrack::LatestPoint() const { + if (empty()) return boost::none; + return points_.crbegin()->second; +} + +boost::optional FeatureTrack::PreviousTimestamp() const { + if (size() < 2) return boost::none; + return std::next(points_.crbegin())->first; +} + +boost::optional FeatureTrack::LatestTimestamp() const { + if (empty()) return boost::none; + return points_.crbegin()->first; +} + +boost::optional FeatureTrack::OldestTimestamp() const { + if (empty()) return boost::none; + return points_.cbegin()->first; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_tracker.cc b/localization/graph_localizer/src/feature_tracker.cc index 4a18ea46d3..01582bc795 100644 --- a/localization/graph_localizer/src/feature_tracker.cc +++ b/localization/graph_localizer/src/feature_tracker.cc @@ -24,96 +24,125 @@ namespace lc = localization_common; namespace lm = localization_measurements; FeatureTracker::FeatureTracker(const FeatureTrackerParams& params) : params_(params) {} void FeatureTracker::UpdateFeatureTracks(const lm::FeaturePoints& feature_points) { - // Update latest time if a new time is available. - // Feature points don't necessarily arrive in time order. - if (!feature_points.empty()) { - latest_time_ = - latest_time_ ? std::max(*latest_time_, feature_points.front().timestamp) : feature_points.front().timestamp; + if (feature_points.empty()) { + Clear(); + LogDebug("UpdateFeatureTracks: Removed all feature tracks."); + return; } - const int starting_num_feature_tracks = feature_tracks_.size(); - + const int starting_num_feature_tracks = size(); LogDebug("UpdateFeatureTracks: Starting num feature tracks: " << starting_num_feature_tracks); - // Update existing features or add new one for (const auto& feature_point : feature_points) { - feature_tracks_[feature_point.feature_id].latest_image_id = feature_point.image_id; - feature_tracks_[feature_point.feature_id].id = feature_point.feature_id; - feature_tracks_[feature_point.feature_id].points.emplace_back(feature_point); + AddOrUpdateTrack(feature_point); } - - const int post_add_num_feature_tracks = feature_tracks_.size(); + const int post_add_num_feature_tracks = size(); LogDebug("UpdateFeatureTracks: Added feature tracks: " << post_add_num_feature_tracks - starting_num_feature_tracks); // Remove features that weren't detected - const auto image_id = feature_points.empty() ? 0 : feature_points.front().image_id; - const bool remove_all_features = feature_points.empty(); - for (auto feature_it = feature_tracks_.cbegin(); feature_it != feature_tracks_.cend();) { - if (feature_it->second.latest_image_id != image_id || remove_all_features) { - feature_it = feature_tracks_.erase(feature_it); + const auto feature_points_timestamp = feature_points.front().timestamp; + RemoveUndetectedFeatures(feature_points_timestamp); + UpdateLengthMap(); + UpdateAllowList(feature_points.front().timestamp); + const int removed_num_feature_tracks = post_add_num_feature_tracks - size(); + LogDebug("UpdateFeatureTracks: Removed feature tracks: " << removed_num_feature_tracks); + LogDebug("UpdateFeatureTracks: Final total num feature tracks: " << size()); +} + +void FeatureTracker::UpdateAllowList(const lc::Time& timestamp) { + // Space out optical flow measurements for smart factor adder if necessary + static int measurement_count = 0; + if (measurement_count++ % (params_.smart_projection_adder_measurement_spacing + 1) != 0) return; + smart_factor_timestamp_allow_list_.emplace(timestamp); +} + +void FeatureTracker::SlideAllowList(const lc::Time& oldest_allowed_time) { + smart_factor_timestamp_allow_list_.erase(smart_factor_timestamp_allow_list_.begin(), + smart_factor_timestamp_allow_list_.lower_bound(oldest_allowed_time)); +} + +void FeatureTracker::RemoveOldFeaturePointsAndSlideWindow(boost::optional oldest_allowed_time) { + const auto latest_time = LatestTimestamp(); + if (!latest_time) return; + const lc::Time window_start = *latest_time - params_.sliding_window_duration; + if (window_start <= 0 && !oldest_allowed_time) return; + oldest_allowed_time = oldest_allowed_time ? std::max(*oldest_allowed_time, window_start) : window_start; + + for (auto feature_track : feature_track_id_map_) { + feature_track.second->RemoveOldMeasurements(*oldest_allowed_time); + } + + SlideAllowList(*oldest_allowed_time); + UpdateLengthMap(); +} + +void FeatureTracker::RemoveUndetectedFeatures(const lc::Time& feature_point_timestamp) { + for (auto feature_it = feature_track_id_map_.cbegin(); feature_it != feature_track_id_map_.cend();) { + if (!feature_it->second->HasMeasurement(feature_point_timestamp)) { + feature_it = feature_track_id_map_.erase(feature_it); } else { ++feature_it; } } - - const int removed_num_feature_tracks = post_add_num_feature_tracks - feature_tracks_.size(); - LogDebug("UpdateFeatureTracks: Removed feature tracks: " << removed_num_feature_tracks); - LogDebug("UpdateFeatureTracks: Final total num feature tracks: " << feature_tracks_.size()); } -void FeatureTracker::RemovePointsOutsideWindow() { - if (!latest_time_) return; - const lc::Time oldest_allowed_time = *latest_time_ - params_.sliding_window_duration; - if (oldest_allowed_time <= 0) return; - RemoveOldFeaturePoints(oldest_allowed_time); +void FeatureTracker::AddOrUpdateTrack(const lm::FeaturePoint& feature_point) { + if (feature_track_id_map_.count(feature_point.feature_id) == 0) { + feature_track_id_map_[feature_point.feature_id] = std::make_shared(feature_point.feature_id); + } + feature_track_id_map_[feature_point.feature_id]->AddMeasurement(feature_point.timestamp, feature_point); } -void FeatureTracker::RemoveOldFeaturePoints(lc::Time oldest_allowed_time) { - // Remove any timestamp before oldest_allowed_time and before start of time window - oldest_allowed_time = - latest_time_ ? std::max(*latest_time_ - params_.sliding_window_duration, oldest_allowed_time) : oldest_allowed_time; - - // Handle any out of order tracks that are too old. Split into two for loops - // so ordered points can be removed in sub linear time (most measurements are ordered). - // TODO(rsoussan): Do this more efficiently - for (auto& feature : feature_tracks_) { - auto point_it = feature.second.points.cbegin(); - while (point_it != feature.second.points.cend() && point_it->timestamp < oldest_allowed_time) { - ++point_it; - } - feature.second.points.erase(feature.second.points.begin(), point_it); +void FeatureTracker::UpdateLengthMap() { + feature_track_length_map_.clear(); + for (const auto& feature_track : feature_track_id_map_) { + feature_track_length_map_.emplace(feature_track.second->size(), feature_track.second); } +} - for (auto& feature : feature_tracks_) { - auto point_it = feature.second.points.cbegin(); - while (point_it != feature.second.points.cend()) { - if (point_it->timestamp < oldest_allowed_time) - point_it = feature.second.points.erase(point_it); - else - ++point_it; - } - } +const FeatureTrackIdMap& FeatureTracker::feature_tracks() const { return feature_track_id_map_; } + +const std::set& FeatureTracker::smart_factor_timestamp_allow_list() const { + return smart_factor_timestamp_allow_list_; } -boost::optional FeatureTracker::latest_timestamp() const { return latest_time_; } +const FeatureTrackLengthMap& FeatureTracker::feature_tracks_length_ordered() const { return feature_track_length_map_; } + +int FeatureTracker::NumTracksWithAtLeastNPoints(int n) const { + const auto lower_bound_it = feature_track_length_map_.lower_bound(n); + return std::distance(lower_bound_it, feature_track_length_map_.end()); +} + +size_t FeatureTracker::size() const { return feature_track_id_map_.size(); } + +bool FeatureTracker::empty() const { return feature_track_id_map_.empty(); } + +void FeatureTracker::Clear() { + feature_track_id_map_.clear(); + feature_track_length_map_.clear(); + smart_factor_timestamp_allow_list_.clear(); +} + +boost::optional FeatureTracker::LatestTimestamp() const { + if (empty()) return boost::none; + // Since Feature Tracks without latest timestamp are erased on updates, each track contains the latest timestamp + return feature_track_id_map_.cbegin()->second->LatestTimestamp(); +} boost::optional FeatureTracker::PreviousTimestamp() const { - for (const auto& feature_track_pair : feature_tracks_) { - const auto& points = feature_track_pair.second.points; - if (points.size() < 2) continue; - return points[points.size() - 2].timestamp; - } - return boost::none; + const auto& longest_feature_track = LongestFeatureTrack(); + if (!longest_feature_track) return boost::none; + // TODO(rsoussan): Need to check this before returning? If boost::none, is this cast correctly when returned here? + return longest_feature_track->PreviousTimestamp(); } -// TODO(rsoussan): Store points in sorted order to make this and PreviousTimestamp more efficient boost::optional FeatureTracker::OldestTimestamp() const { - boost::optional oldest_timestamp; - for (const auto& feature_track_pair : feature_tracks_) { - for (const auto& point : feature_track_pair.second.points) { - if (!oldest_timestamp || point.timestamp < *oldest_timestamp) oldest_timestamp = point.timestamp; - } - } - return oldest_timestamp; + const auto& longest_feature_track = LongestFeatureTrack(); + if (!longest_feature_track) return boost::none; + return longest_feature_track->OldestTimestamp(); } +boost::optional FeatureTracker::LongestFeatureTrack() const { + if (empty()) return boost::none; + return *(feature_track_length_map_.rbegin()->second.get()); +} } // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer.cc b/localization/graph_localizer/src/graph_localizer.cc index 899c439ee5..89b7c6c067 100644 --- a/localization/graph_localizer/src/graph_localizer.cc +++ b/localization/graph_localizer/src/graph_localizer.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -40,143 +41,75 @@ #include #include -namespace { -// TODO(rsoussan): Is this necessary? Just use DFATAL and compile with debug? -// Avoid having to compile with DEBUG to toggle between fatal and non-fatal failures -void log(const bool fatal_failure, const std::string& description) { - if (fatal_failure) { - LogFatal(description); - } else { - LogError(description); - } -} -} // namespace - namespace graph_localizer { +namespace go = graph_optimizer; namespace ii = imu_integration; namespace lc = localization_common; namespace lm = localization_measurements; GraphLocalizer::GraphLocalizer(const GraphLocalizerParams& params) - : feature_tracker_(new FeatureTracker(params.feature_tracker)), - standstill_feature_tracker_(new FeatureTracker(params.standstill_feature_tracker)), - latest_imu_integrator_(params.graph_initializer), - graph_values_(new GraphValues(params.graph_values)), - log_on_destruction_(true), + : GraphOptimizer(params.graph_optimizer, std::unique_ptr(new GraphLocalizerStats())), + feature_tracker_(new FeatureTracker(params.feature_tracker)), + latest_imu_integrator_(new ii::LatestImuIntegrator(params.graph_initializer)), params_(params) { - latest_imu_integrator_.SetFanSpeedMode(params_.initial_fan_speed_mode); + latest_imu_integrator_->SetFanSpeedMode(params_.initial_fan_speed_mode); + + // Initialize Node Updaters // Assumes zero initial velocity const lc::CombinedNavState global_N_body_start(params_.graph_initializer.global_T_body_start, gtsam::Velocity3::Zero(), params_.graph_initializer.initial_imu_bias, params_.graph_initializer.start_time); + params_.combined_nav_state_node_updater.global_N_body_start = global_N_body_start; + combined_nav_state_node_updater_.reset( + new CombinedNavStateNodeUpdater(params_.combined_nav_state_node_updater, latest_imu_integrator_, values())); + combined_nav_state_node_updater_->AddInitialValuesAndPriors(graph_factors()); + AddNodeUpdater(combined_nav_state_node_updater_); + // TODO(rsoussan): Clean this up + dynamic_cast(graph_stats()) + ->SetCombinedNavStateGraphValues(combined_nav_state_node_updater_->shared_graph_values()); - // Add first nav state and priors to graph - const int key_index = GenerateKeyIndex(); - graph_values_->AddCombinedNavState(global_N_body_start, key_index); - AddStartingPriors(global_N_body_start, key_index, graph_); - - // Initialize smart projection factor params - // TODO(rsoussan): Remove this once splitting function is moved, remove smart_projection_params_ from graph_localizer - smart_projection_params_.verboseCheirality = params_.factor.smart_projection_adder.verbose_cheirality; - smart_projection_params_.setRankTolerance(1e-9); - smart_projection_params_.setLandmarkDistanceThreshold( - params_.factor.smart_projection_adder.landmark_distance_threshold); - smart_projection_params_.setDynamicOutlierRejectionThreshold( - params_.factor.smart_projection_adder.dynamic_outlier_rejection_threshold); - smart_projection_params_.setRetriangulationThreshold(params_.factor.smart_projection_adder.retriangulation_threshold); - smart_projection_params_.setEnableEPI(params_.factor.smart_projection_adder.enable_EPI); - - // Initialize projection triangulation params - projection_triangulation_params_.rankTolerance = 1e-9; - projection_triangulation_params_.enableEPI = params_.factor.projection_adder.enable_EPI; - projection_triangulation_params_.landmarkDistanceThreshold = - params_.factor.projection_adder.landmark_distance_threshold; - projection_triangulation_params_.dynamicOutlierRejectionThreshold = - params_.factor.projection_adder.dynamic_outlier_rejection_threshold; - - // Initialize lm params - if (params_.verbose) { - levenberg_marquardt_params_.verbosityLM = gtsam::LevenbergMarquardtParams::VerbosityLM::TRYDELTA; - levenberg_marquardt_params_.verbosity = gtsam::NonlinearOptimizerParams::Verbosity::LINEAR; - } - if (params_.use_ceres_params) { - gtsam::LevenbergMarquardtParams::SetCeresDefaults(&levenberg_marquardt_params_); - } - - levenberg_marquardt_params_.maxIterations = params_.max_iterations; - - if (params_.marginals_factorization == "qr") { - marginals_factorization_ = gtsam::Marginals::Factorization::QR; - } else if (params_.marginals_factorization == "cholesky") { - marginals_factorization_ = gtsam::Marginals::Factorization::CHOLESKY; - } else { - LogError("GraphLocalizer: No marginals factorization entered, defaulting to qr."); - marginals_factorization_ = gtsam::Marginals::Factorization::QR; - } + feature_point_node_updater_.reset(new FeaturePointNodeUpdater(params.feature_point_node_updater, values())); + AddNodeUpdater(feature_point_node_updater_); // Initialize Factor Adders ar_tag_loc_factor_adder_.reset( - new LocFactorAdder(params_.factor.ar_tag_loc_adder, GraphAction::kARTagProjectionNoiseScaling)); - loc_factor_adder_.reset(new LocFactorAdder(params_.factor.loc_adder, GraphAction::kLocProjectionNoiseScaling)); + new LocFactorAdder(params_.factor.ar_tag_loc_adder, go::GraphActionCompleterType::ARTagLocProjectionFactor)); + loc_factor_adder_.reset( + new LocFactorAdder(params_.factor.loc_adder, go::GraphActionCompleterType::LocProjectionFactor)); projection_factor_adder_.reset( - new ProjectionFactorAdder(params_.factor.projection_adder, feature_tracker_, graph_values_)); + new ProjectionFactorAdder(params_.factor.projection_adder, feature_tracker_, + feature_point_node_updater_->shared_feature_point_graph_values())); rotation_factor_adder_.reset(new RotationFactorAdder(params_.factor.rotation_adder, feature_tracker_)); smart_projection_cumulative_factor_adder_.reset( new SmartProjectionCumulativeFactorAdder(params_.factor.smart_projection_adder, feature_tracker_)); standstill_factor_adder_.reset(new StandstillFactorAdder(params_.factor.standstill_adder, feature_tracker_)); -} - -GraphLocalizer::~GraphLocalizer() { - if (log_on_destruction_) graph_stats_.Log(); -} -void GraphLocalizer::AddStartingPriors(const lc::CombinedNavState& global_N_body_start, const int key_index, - gtsam::NonlinearFactorGraph& graph) { - const gtsam::Vector6 pose_prior_noise_sigmas( - (gtsam::Vector(6) << params_.noise.starting_prior_translation_stddev, - params_.noise.starting_prior_translation_stddev, params_.noise.starting_prior_translation_stddev, - params_.noise.starting_prior_quaternion_stddev, params_.noise.starting_prior_quaternion_stddev, - params_.noise.starting_prior_quaternion_stddev) - .finished()); - const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params_.noise.starting_prior_velocity_stddev, - params_.noise.starting_prior_velocity_stddev, - params_.noise.starting_prior_velocity_stddev) - .finished()); - const gtsam::Vector6 bias_prior_noise_sigmas( - (gtsam::Vector(6) << params_.noise.starting_prior_accel_bias_stddev, params_.noise.starting_prior_accel_bias_stddev, - params_.noise.starting_prior_accel_bias_stddev, params_.noise.starting_prior_gyro_bias_stddev, - params_.noise.starting_prior_gyro_bias_stddev, params_.noise.starting_prior_gyro_bias_stddev) - .finished()); - lc::CombinedNavStateNoise noise; - noise.pose_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_prior_noise_sigmas)), params_.huber_k); - noise.velocity_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), - params_.huber_k); - noise.bias_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(bias_prior_noise_sigmas)), params_.huber_k); - AddPriors(global_N_body_start, noise, key_index, graph); -} - -void GraphLocalizer::AddPriors(const lc::CombinedNavState& global_N_body, const lc::CombinedNavStateNoise& noise, - const int key_index, gtsam::NonlinearFactorGraph& graph) { - gtsam::PriorFactor pose_prior_factor(sym::P(key_index), global_N_body.pose(), noise.pose_noise); - graph.push_back(pose_prior_factor); - gtsam::PriorFactor velocity_prior_factor(sym::V(key_index), global_N_body.velocity(), - noise.velocity_noise); - graph.push_back(velocity_prior_factor); - gtsam::PriorFactor bias_prior_factor(sym::B(key_index), global_N_body.bias(), - noise.bias_noise); - graph.push_back(bias_prior_factor); + // Initialize Graph Action Completers + ar_tag_loc_graph_action_completer_.reset( + new LocGraphActionCompleter(params_.factor.ar_tag_loc_adder, go::GraphActionCompleterType::ARTagLocProjectionFactor, + combined_nav_state_node_updater_->shared_graph_values())); + AddGraphActionCompleter(ar_tag_loc_graph_action_completer_); + + loc_graph_action_completer_.reset( + new LocGraphActionCompleter(params_.factor.loc_adder, go::GraphActionCompleterType::LocProjectionFactor, + combined_nav_state_node_updater_->shared_graph_values())); + AddGraphActionCompleter(loc_graph_action_completer_); + projection_graph_action_completer_.reset(new ProjectionGraphActionCompleter( + params_.factor.projection_adder, combined_nav_state_node_updater_->shared_graph_values(), + feature_point_node_updater_->shared_feature_point_graph_values())); + AddGraphActionCompleter(projection_graph_action_completer_); + smart_projection_graph_action_completer_.reset(new SmartProjectionGraphActionCompleter( + params_.factor.smart_projection_adder, combined_nav_state_node_updater_->shared_graph_values())); + AddGraphActionCompleter(smart_projection_graph_action_completer_); } boost::optional> GraphLocalizer::LatestCombinedNavStateAndCovariances() const { - if (!marginals_) { + if (!marginals()) { LogDebugEveryN(50, "LatestCombinedNavStateAndCovariances: No marginals available."); return boost::none; } - const auto state_covariance_pair = LatestCombinedNavStateAndCovariances(*marginals_); + const auto state_covariance_pair = LatestCombinedNavStateAndCovariances(*marginals()); if (!state_covariance_pair) { LogDebug( "LatestCombinedNavStateAndCovariances: Failed to get latest combined nav state and " @@ -189,12 +122,13 @@ GraphLocalizer::LatestCombinedNavStateAndCovariances() const { boost::optional> GraphLocalizer::LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const { - const auto global_N_body_latest = graph_values_->LatestCombinedNavState(); + const auto global_N_body_latest = combined_nav_state_node_updater_->graph_values().LatestCombinedNavState(); if (!global_N_body_latest) { LogError("LatestCombinedNavStateAndCovariance: Failed to get latest combined nav state."); return boost::none; } - const auto latest_combined_nav_state_key_index = graph_values_->LatestCombinedNavStateKeyIndex(); + const auto latest_combined_nav_state_key_index = + combined_nav_state_node_updater_->graph_values().LatestCombinedNavStateKeyIndex(); if (!latest_combined_nav_state_key_index) { LogError("LatestCombinedNavStateAndCovariance: Failed to get latest combined nav state."); return boost::none; @@ -215,7 +149,7 @@ GraphLocalizer::LatestCombinedNavStateAndCovariances(const gtsam::Marginals& mar } boost::optional GraphLocalizer::LatestCombinedNavState() const { - const auto global_N_body_latest = graph_values_->LatestCombinedNavState(); + const auto global_N_body_latest = combined_nav_state_node_updater_->graph_values().LatestCombinedNavState(); if (!global_N_body_latest) { LogError("LatestCombinedNavState: Failed to get latest combined nav state."); return boost::none; @@ -225,7 +159,8 @@ boost::optional GraphLocalizer::LatestCombinedNavState() c } boost::optional GraphLocalizer::GetCombinedNavState(const lc::Time time) const { - const auto lower_bound_or_equal_combined_nav_state = graph_values_->LowerBoundOrEqualCombinedNavState(time); + const auto lower_bound_or_equal_combined_nav_state = + combined_nav_state_node_updater_->graph_values().LowerBoundOrEqualCombinedNavState(time); if (!lower_bound_or_equal_combined_nav_state) { LogDebug("GetCombinedNavState: Failed to get lower bound or equal combined nav state."); return boost::none; @@ -237,9 +172,9 @@ boost::optional GraphLocalizer::GetCombinedNavState(const // Pim predict from lower bound state rather than closest state so there is no // need to reverse predict (going backwards in time) using a pim prediction which is not yet supported in gtsam. - auto integrated_pim = latest_imu_integrator_.IntegratedPim(lower_bound_or_equal_combined_nav_state->bias(), - lower_bound_or_equal_combined_nav_state->timestamp(), time, - latest_imu_integrator_.pim_params()); + auto integrated_pim = latest_imu_integrator_->IntegratedPim(lower_bound_or_equal_combined_nav_state->bias(), + lower_bound_or_equal_combined_nav_state->timestamp(), + time, latest_imu_integrator_->pim_params()); if (!integrated_pim) { LogError("GetCombinedNavState: Failed to create integrated pim."); return boost::none; @@ -249,7 +184,7 @@ boost::optional GraphLocalizer::GetCombinedNavState(const } boost::optional> GraphLocalizer::LatestBiases() const { - const auto latest_bias = graph_values_->LatestBias(); + const auto latest_bias = combined_nav_state_node_updater_->graph_values().LatestBias(); if (!latest_bias) { LogError("LatestBiases: Failed to get latest biases."); return boost::none; @@ -259,11 +194,11 @@ boost::optional> GraphLocalize // Latest extrapolated pose time is limited by latest imu time boost::optional GraphLocalizer::LatestExtrapolatedPoseTime() const { - return latest_imu_integrator_.LatestTime(); + return latest_imu_integrator_->LatestTime(); } void GraphLocalizer::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { - latest_imu_integrator_.BufferImuMeasurement(imu_measurement); + latest_imu_integrator_->BufferImuMeasurement(imu_measurement); } bool GraphLocalizer::AddOpticalFlowMeasurement( @@ -282,15 +217,6 @@ bool GraphLocalizer::AddOpticalFlowMeasurement( } last_time = optical_flow_feature_points_measurement.timestamp; - CheckForStandstill(optical_flow_feature_points_measurement); - if (standstill() && params_.factor.standstill_adder.enabled) { - BufferFactors(standstill_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); - } - - // Skip optical flow measurements if necessary - static int optical_flow_measurement_count = 0; - if (optical_flow_measurement_count++ % (params_.optical_flow_measurement_spacing + 1) != 0) return false; - LogDebug("AddOpticalFlowMeasurement: Adding optical flow measurement."); feature_tracker_->UpdateFeatureTracks(optical_flow_feature_points_measurement.feature_points); @@ -307,18 +233,23 @@ bool GraphLocalizer::AddOpticalFlowMeasurement( BufferFactors(rotation_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); } + CheckForStandstill(); + if (standstill() && params_.factor.standstill_adder.enabled) { + BufferFactors(standstill_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); + } + return true; } -void GraphLocalizer::CheckForStandstill(const lm::FeaturePointsMeasurement& optical_flow_feature_points_measurement) { - standstill_feature_tracker_->UpdateFeatureTracks(optical_flow_feature_points_measurement.feature_points); +void GraphLocalizer::CheckForStandstill() { // Check for standstill via low disparity for all feature tracks double total_average_distance_from_mean = 0; int num_valid_feature_tracks = 0; - for (const auto& feature_track : standstill_feature_tracker_->feature_tracks()) { - const double average_distance_from_mean = AverageDistanceFromMean(feature_track.second.points); + for (const auto& feature_track : feature_tracker_->feature_tracks()) { + const double average_distance_from_mean = + AverageDistanceFromMean(feature_track.second->LatestPointsInWindow(params_.standstill_feature_track_duration)); // Only consider long enough feature tracks for standstill candidates - if (static_cast(feature_track.second.points.size()) >= params_.standstill_min_num_points_per_track) { + if (static_cast(feature_track.second->size()) >= params_.standstill_min_num_points_per_track) { total_average_distance_from_mean += average_distance_from_mean; ++num_valid_feature_tracks; } @@ -360,519 +291,22 @@ void GraphLocalizer::AddSparseMappingMeasurement( } } -// TODO(rsoussan): Clean this function up (duplicate code), address other todo's -void GraphLocalizer::SplitSmartFactorsIfNeeded(FactorsToAdd& factors_to_add) { - for (auto& factor_to_add : factors_to_add.Get()) { - auto smart_factor = dynamic_cast(factor_to_add.factor.get()); - if (!smart_factor) continue; - // Can't remove measurements if there are only 2 or fewer - if (smart_factor->measured().size() <= 2) continue; - const auto point = smart_factor->triangulateSafe(smart_factor->cameras(graph_values_->values())); - if (point.valid()) continue; - { - const auto fixed_smart_factor = FixSmartFactorByRemovingIndividualMeasurements( - params_, *smart_factor, smart_projection_params_, *graph_values_); - if (fixed_smart_factor) { - factor_to_add.factor = *fixed_smart_factor; - continue; - } - } - { - const auto fixed_smart_factor = - FixSmartFactorByRemovingMeasurementSequence(params_, *smart_factor, smart_projection_params_, *graph_values_); - if (fixed_smart_factor) { - factor_to_add.factor = *fixed_smart_factor; - continue; - } - } - LogDebug("SplitSmartFactorsIfNeeded: Failed to fix smart factor"); - } -} - -bool GraphLocalizer::TriangulateNewPoint(FactorsToAdd& factors_to_add) { - gtsam::CameraSet camera_set; - Camera::MeasurementVector measurements; - gtsam::Key point_key; - for (const auto& factor_to_add : factors_to_add.Get()) { - const auto& factor = factor_to_add.factor; - const auto projection_factor = dynamic_cast(factor.get()); - if (!projection_factor) { - LogError("TriangulateNewPoint: Failed to cast to projection factor."); - return false; - } - const auto world_T_body = graph_values_->at(projection_factor->key1()); - if (!world_T_body) { - LogError("TriangulateNewPoint: Failed to get pose."); - return false; - } - - const gtsam::Pose3 world_T_camera = *world_T_body * params_.calibration.body_T_nav_cam; - camera_set.emplace_back(world_T_camera, params_.calibration.nav_cam_intrinsics); - measurements.emplace_back(projection_factor->measured()); - point_key = projection_factor->key2(); - } - gtsam::TriangulationResult world_t_triangulated_point; - // TODO(rsoussan): Gtsam shouldn't be throwing exceptions for this, but needed if enable_epi enabled. - // Is there a build setting that prevents cheirality from being thrown in this case? - try { - world_t_triangulated_point = gtsam::triangulateSafe(camera_set, measurements, projection_triangulation_params_); - } catch (...) { - LogDebug("TriangulateNewPoint: Exception occurred during triangulation"); - return false; - } - - if (!world_t_triangulated_point.valid()) { - LogDebug("TriangulateNewPoint: Failed to triangulate point"); - return false; - } - // TODO(rsoussan): clean this up - const auto feature_id = factors_to_add.Get().front().key_infos[1].id(); - graph_values_->AddFeature(feature_id, *world_t_triangulated_point, point_key); - if (params_.factor.projection_adder.add_point_priors) { - const gtsam::Vector3 point_prior_noise_sigmas((gtsam::Vector(3) << params_.noise.point_prior_translation_stddev, - params_.noise.point_prior_translation_stddev, - params_.noise.point_prior_translation_stddev) - .finished()); - const auto point_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(point_prior_noise_sigmas)), - params_.huber_k); - gtsam::PriorFactor point_prior_factor(point_key, *world_t_triangulated_point, point_noise); - graph_.push_back(point_prior_factor); - } - return true; -} - -bool GraphLocalizer::LocProjectionNoiseScaling(FactorsToAdd& factors_to_add) { - return MapProjectionNoiseScaling(params_.factor.loc_adder, factors_to_add); -} - -bool GraphLocalizer::ARProjectionNoiseScaling(FactorsToAdd& factors_to_add) { - return MapProjectionNoiseScaling(params_.factor.ar_tag_loc_adder, factors_to_add); -} - -bool GraphLocalizer::MapProjectionNoiseScaling(const LocFactorAdderParams& params, FactorsToAdd& factors_to_add) { - auto& factors = factors_to_add.Get(); - boost::optional pose_key; - boost::optional world_T_cam; - for (auto factor_it = factors.begin(); factor_it != factors.end();) { - auto& factor = factor_it->factor; - auto projection_factor = dynamic_cast*>(factor.get()); - if (!projection_factor) { - LogError("MapProjectionNoiseScaling: Failed to cast to projection factor."); - return false; - } - - // Save pose and pose key in case need to make loc prior - pose_key = projection_factor->key(); - world_T_cam = projection_factor->world_T_cam(); - - const auto world_T_body = graph_values_->at(projection_factor->key()); - if (!world_T_body) { - LogError("MapProjectionNoiseScaling: Failed to get pose."); - return false; - } - const auto error = (projection_factor->evaluateError(*world_T_body)).norm(); - const auto cheirality_error = projection_factor->cheiralityError(*world_T_body); - if (cheirality_error) { - factor_it = factors.erase(factor_it); - } else if (error > params.max_inlier_weighted_projection_norm) { - factor_it = factors.erase(factor_it); - } else { - if (params.weight_projections_with_distance) { - const gtsam::Point3 nav_cam_t_landmark = - (*world_T_body * *(projection_factor->body_P_sensor())).inverse() * projection_factor->landmark_point(); - const gtsam::SharedIsotropic scaled_noise( - gtsam::noiseModel::Isotropic::Sigma(2, params.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); - // Don't use robust cost here to more effectively correct a drift occurance - gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor(new gtsam::LocProjectionFactor<>( - projection_factor->measured(), projection_factor->landmark_point(), scaled_noise, projection_factor->key(), - projection_factor->calibration(), *(projection_factor->body_P_sensor()))); - factor_it->factor = loc_projection_factor; - } - ++factor_it; - } - } - // All factors have been removed due to errors, use loc pose prior instead - if (factors.empty() && params.add_prior_if_projections_fail) { - if (!pose_key || !world_T_cam) { - LogError("MapProjectionNoiseScaling: Failed to get pose key and world_T_cam"); - return false; - } - const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << params.prior_translation_stddev, - params.prior_translation_stddev, params.prior_translation_stddev, - params.prior_quaternion_stddev, params.prior_quaternion_stddev, - params.prior_quaternion_stddev) - .finished()); - // TODO(rsoussan): enable scaling with num landmarks - const int noise_scale = 1; - const auto pose_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas)), - params.huber_k); - gtsam::LocPoseFactor::shared_ptr pose_prior_factor( - new gtsam::LocPoseFactor(*pose_key, *world_T_cam * params.body_T_cam.inverse(), pose_noise)); - factors_to_add.push_back(FactorToAdd({KeyInfo(&sym::P, factors_to_add.timestamp())}, pose_prior_factor)); - } - return true; -} - -bool GraphLocalizer::AddOrSplitImuFactorIfNeeded(const lc::Time timestamp) { - if (graph_values_->HasKey(timestamp)) { - LogDebug( - "AddOrSplitImuFactorIfNeeded: CombinedNavState exists at " - "timestamp, nothing to do."); - return true; - } - - const auto latest_timestamp = graph_values_->LatestTimestamp(); - if (!latest_timestamp) { - LogError("AddOrSplitImuFactorIfNeeded: Failed to get latest timestamp."); - return false; - } - - if (timestamp > *latest_timestamp) { - LogDebug( - "AddOrSplitImuFactorIfNeeded: Creating and adding latest imu " - "factor and nav state."); - const auto timestamps_to_add = TimestampsToAdd(timestamp, *latest_timestamp); - if (timestamps_to_add.size() > 1) - LogDebug("AddOrSplitImuFactorIfNeeded: Adding extra imu factors and nav states due to large time difference."); - bool added_timestamps = true; - for (const auto timestamp_to_add : timestamps_to_add) { - added_timestamps &= CreateAndAddLatestImuFactorAndCombinedNavState(timestamp_to_add); - } - return added_timestamps; - } else { - LogDebug("AddOrSplitImuFactorIfNeeded: Splitting old imu factor."); - return SplitOldImuFactorAndAddCombinedNavState(timestamp); - } -} - -std::vector GraphLocalizer::TimestampsToAdd(const lc::Time timestamp, const lc::Time last_added_timestamp) { - if (!params_.limit_imu_factor_spacing) return {timestamp}; - const double timestamp_difference = timestamp - last_added_timestamp; - if (timestamp_difference < params_.max_imu_factor_spacing) return {timestamp}; - // Evenly distribute timestamps so that the min number is added such that each spacing is <= max_imu_factor_spacing - const int num_timestamps_to_add = std::ceil(timestamp_difference / params_.max_imu_factor_spacing); - const double timestamps_to_add_spacing = timestamp_difference / num_timestamps_to_add; - std::vector timestamps_to_add; - // Add up to final timestamp, insert timestamp argument as final timestamp to ensure no floating point errors occur, - // since the final timestamp should exactly match the given timestamp - for (int i = 1; i < num_timestamps_to_add; ++i) { - const double timestamp_to_add = last_added_timestamp + i * timestamps_to_add_spacing; - // TODO(rsoussan): Account for recent enough when calculating spacing? - if (MeasurementRecentEnough(timestamp_to_add)) timestamps_to_add.emplace_back(timestamp_to_add); - } - timestamps_to_add.emplace_back(timestamp); - - return timestamps_to_add; -} - -bool GraphLocalizer::SplitOldImuFactorAndAddCombinedNavState(const lc::Time timestamp) { - const auto timestamp_bounds = graph_values_->LowerAndUpperBoundTimestamp(timestamp); - if (!timestamp_bounds.first || !timestamp_bounds.second) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get upper and lower bound timestamp."); - return false; - } - - const lc::Time lower_bound_time = *(timestamp_bounds.first); - const lc::Time upper_bound_time = *(timestamp_bounds.second); - - if (timestamp < lower_bound_time || timestamp > upper_bound_time) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Timestamp is not within bounds of existing timestamps."); - return false; - } - - const auto lower_bound_key_index = graph_values_->KeyIndex(lower_bound_time); - const auto upper_bound_key_index = graph_values_->KeyIndex(upper_bound_time); - if (!lower_bound_key_index || !upper_bound_key_index) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower and upper bound key indices."); - return false; - } - - // get old imu factor, delete it - bool removed_old_imu_factor = false; - for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { - if (dynamic_cast(factor_it->get()) && - graph_values_->ContainsCombinedNavStateKey(**factor_it, *lower_bound_key_index) && - graph_values_->ContainsCombinedNavStateKey(**factor_it, *upper_bound_key_index)) { - graph_.erase(factor_it); - removed_old_imu_factor = true; - break; - } - ++factor_it; - } - if (!removed_old_imu_factor) { - LogError( - "SplitOldImuFactorAndAddCombinedNavState: Failed to remove " - "old imu factor."); - return false; - } - - const auto lower_bound_bias = graph_values_->at(sym::B(*lower_bound_key_index)); - if (!lower_bound_bias) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound bias."); - return false; - } - - // Add first factor and new nav state at timestamp - auto first_integrated_pim = latest_imu_integrator_.IntegratedPim(*lower_bound_bias, lower_bound_time, timestamp, - latest_imu_integrator_.pim_params()); - if (!first_integrated_pim) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create first integrated pim."); - return false; - } - - const auto lower_bound_combined_nav_state = graph_values_->GetCombinedNavState(lower_bound_time); - if (!lower_bound_combined_nav_state) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound combined nav state."); - return false; - } - - if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*lower_bound_combined_nav_state, *first_integrated_pim)) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create and add imu factor."); - return false; - } - - // Add second factor, use lower_bound_bias as starting bias since that is the - // best estimate available - auto second_integrated_pim = latest_imu_integrator_.IntegratedPim(*lower_bound_bias, timestamp, upper_bound_time, - latest_imu_integrator_.pim_params()); - if (!second_integrated_pim) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create second integrated pim."); - return false; - } - - // New nav state already added so just get its key index - const auto new_key_index = graph_values_->KeyIndex(timestamp); - if (!new_key_index) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get new key index."); - return false; - } - - const auto combined_imu_factor = - ii::MakeCombinedImuFactor(*new_key_index, *upper_bound_key_index, *second_integrated_pim); - graph_.push_back(combined_imu_factor); - return true; -} - -bool GraphLocalizer::CreateAndAddLatestImuFactorAndCombinedNavState(const lc::Time timestamp) { - if (!latest_imu_integrator_.IntegrateLatestImuMeasurements(timestamp)) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to integrate latest imu measurements."); - return false; - } - - const auto latest_combined_nav_state = graph_values_->LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest combined nav state."); - return false; - } - if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*latest_combined_nav_state, latest_imu_integrator_.pim())) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to create and add imu factor."); - return false; - } - - const auto latest_bias = graph_values_->LatestBias(); - if (!latest_bias) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest bias."); - return false; - } - - latest_imu_integrator_.ResetPimIntegrationAndSetBias(latest_bias->first); - return true; -} - -bool GraphLocalizer::CreateAndAddImuFactorAndPredictedCombinedNavState( - const lc::CombinedNavState& global_N_body, const gtsam::PreintegratedCombinedMeasurements& pim) { - const auto key_index_0 = graph_values_->KeyIndex(global_N_body.timestamp()); - if (!key_index_0) { - LogError("CreateAndAddImuFactorAndPredictedCombinedNavState: Failed to get first key index."); - return false; - } - - const lc::CombinedNavState global_N_body_predicted = ii::PimPredict(global_N_body, pim); - const int key_index_1 = GenerateKeyIndex(); - const auto combined_imu_factor = ii::MakeCombinedImuFactor(*key_index_0, key_index_1, pim); - graph_.push_back(combined_imu_factor); - graph_values_->AddCombinedNavState(global_N_body_predicted, key_index_1); - return true; -} - -// Adapted from gtsam::BatchFixedLagSmoother -gtsam::NonlinearFactorGraph GraphLocalizer::MarginalFactors( - const gtsam::NonlinearFactorGraph& old_factors, const gtsam::KeyVector& old_keys, - const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const { - // Old keys not present in old factors. This shouldn't occur. - if (old_keys.size() == 0) { - LogDebug("MarginalFactors: No old keys provided."); - return old_factors; - } - - // Linearize Graph - const auto linearized_graph = old_factors.linearize(graph_values_->values()); - const auto linear_marginal_factors = - *(linearized_graph->eliminatePartialMultifrontal(old_keys, eliminate_function).second); - return gtsam::LinearContainerFactor::ConvertLinearGraph(linear_marginal_factors, graph_values_->values()); -} - -bool GraphLocalizer::SlideWindow(const boost::optional& marginals, const lc::Time last_latest_time) { - const auto graph_values_ideal_new_oldest_time = graph_values_->SlideWindowNewOldestTime(); - if (!graph_values_ideal_new_oldest_time) { - LogDebug("SlideWindow: No states removed. "); - return true; - } - // Ensure that new oldest time isn't more recent than last latest time - // since then priors couldn't be added for the new oldest state - if (last_latest_time < *graph_values_ideal_new_oldest_time) - LogError("SlideWindow: Ideal oldest time is more recent than last latest time."); - const auto new_oldest_time = std::min(last_latest_time, *graph_values_ideal_new_oldest_time); - - // Add marginal factors for marginalized values - auto old_keys = graph_values_->OldKeys(new_oldest_time); - // Since cumlative factors have many keys and shouldn't be marginalized, need to remove old measurements depending on - // old keys before marginalizing and sliding window - RemoveOldMeasurementsFromCumulativeFactors(old_keys); - auto old_factors = graph_values_->RemoveOldFactors(old_keys, graph_); - gtsam::KeyVector old_feature_keys; - if (params_.factor.projection_adder.enabled) { - // Call remove old factors before old feature keys, since old feature keys depend on - // number of factors per key remaining - old_feature_keys = graph_values_->OldFeatureKeys(graph_); - auto old_feature_factors = graph_values_->RemoveOldFactors(old_feature_keys, graph_); - old_keys.insert(old_keys.end(), old_feature_keys.begin(), old_feature_keys.end()); - old_factors.push_back(old_feature_factors); - } - if (params_.add_marginal_factors) { - const auto marginal_factors = MarginalFactors(old_factors, old_keys, gtsam::EliminateQR); - for (const auto& marginal_factor : marginal_factors) { - graph_.push_back(marginal_factor); - } - } - - graph_values_->RemoveOldCombinedNavStates(new_oldest_time); - if (params_.factor.projection_adder.enabled) graph_values_->RemoveOldFeatures(old_feature_keys); - - // Remove old data from other containers - // TODO(rsoussan): Just use new_oldest_time and don't bother getting oldest timestamp here? - const auto oldest_timestamp = graph_values_->OldestTimestamp(); - if (!oldest_timestamp || *oldest_timestamp != new_oldest_time) { - LogError("SlideWindow: Failed to get oldest timestamp."); - return false; - } - - feature_tracker_->RemoveOldFeaturePoints(*oldest_timestamp); - standstill_feature_tracker_->RemoveOldFeaturePoints(*oldest_timestamp); - latest_imu_integrator_.RemoveOldMeasurements(*oldest_timestamp); - RemoveOldBufferedFactors(*oldest_timestamp); - - if (params_.factor.projection_adder.enabled && params_.factor.projection_adder.add_point_priors && marginals_) { - UpdatePointPriors(*marginals_); - } - - if (params_.add_priors) { - // Add prior to oldest nav state using covariances from last round of - // optimization - const auto global_N_body_oldest = graph_values_->OldestCombinedNavState(); - if (!global_N_body_oldest) { - LogError("SlideWindow: Failed to get oldest combined nav state."); - return false; - } - - LogDebug("SlideWindow: Oldest state time: " << global_N_body_oldest->timestamp()); - - const auto key_index = graph_values_->OldestCombinedNavStateKeyIndex(); - if (!key_index) { - LogError("SlideWindow: Failed to get oldest combined nav state key index."); - return false; - } - - LogDebug("SlideWindow: key index: " << *key_index); - - // Make sure priors are removed before adding new ones - RemovePriors(*key_index); - if (marginals) { - lc::CombinedNavStateNoise noise; - noise.pose_noise = Robust( - gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::P(*key_index))), params_.huber_k); - noise.velocity_noise = Robust( - gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::V(*key_index))), params_.huber_k); - noise.bias_noise = Robust( - gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::B(*key_index))), params_.huber_k); - AddPriors(*global_N_body_oldest, noise, *key_index, graph_); - } else { - // TODO(rsoussan): Add seperate marginal fallback sigmas instead of relying on starting prior sigmas - AddStartingPriors(*global_N_body_oldest, *key_index, graph_); - } - } - - return true; -} - -void GraphLocalizer::UpdatePointPriors(const gtsam::Marginals& marginals) { - const auto feature_keys = graph_values_->FeatureKeys(); - for (const auto& feature_key : feature_keys) { - const auto world_t_point = graph_values_->at(feature_key); - if (!world_t_point) { - LogError("UpdatePointPriors: Failed to get world_t_point."); - continue; - } - for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { - const auto point_prior_factor = dynamic_cast*>(factor_it->get()); - if (point_prior_factor && (point_prior_factor->key() == feature_key)) { - // Erase old prior - factor_it = graph_.erase(factor_it); - // Add updated one - const auto point_prior_noise = - Robust(gtsam::noiseModel::Gaussian::Covariance(marginals.marginalCovariance(feature_key)), params_.huber_k); - const gtsam::PriorFactor point_prior_factor(feature_key, *world_t_point, point_prior_noise); - graph_.push_back(point_prior_factor); - // Only one point prior per feature - break; - } else { - ++factor_it; - } - } - } -} - -void GraphLocalizer::RemovePriors(const int key_index) { - int removed_factors = 0; - for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { - bool erase_factor = false; - const auto pose_prior_factor = dynamic_cast*>(factor_it->get()); - const auto loc_pose_factor = dynamic_cast(factor_it->get()); - if (pose_prior_factor && !loc_pose_factor && pose_prior_factor->key() == sym::P(key_index)) { - erase_factor = true; - } - const auto velocity_prior_factor = dynamic_cast*>(factor_it->get()); - if (velocity_prior_factor && velocity_prior_factor->key() == sym::V(key_index)) { - erase_factor = true; - } - const auto bias_prior_factor = dynamic_cast*>(factor_it->get()); - if (bias_prior_factor && bias_prior_factor->key() == sym::B(key_index)) { - erase_factor = true; - } - - if (erase_factor) { - factor_it = graph_.erase(factor_it); - ++removed_factors; - } else { - ++factor_it; - continue; - } - } - LogDebug("RemovePriors: Erase " << removed_factors << " factors."); +void GraphLocalizer::DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, + const boost::optional& marginals) { + feature_tracker_->RemoveOldFeaturePointsAndSlideWindow(oldest_allowed_time); + latest_imu_integrator_->RemoveOldMeasurements(oldest_allowed_time); } void GraphLocalizer::BufferCumulativeFactors() { // Remove measurements here so they are more likely to fit in sliding window duration when optimized - feature_tracker_->RemovePointsOutsideWindow(); + feature_tracker_->RemoveOldFeaturePointsAndSlideWindow(); if (params_.factor.smart_projection_adder.enabled) { BufferFactors(smart_projection_cumulative_factor_adder_->AddFactors()); } } void GraphLocalizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) { - for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { + for (auto factor_it = graph_factors().begin(); factor_it != graph_factors().end();) { const auto smart_factor = dynamic_cast(factor_it->get()); // Currently the only cumulative factors are smart factors // TODO(rsoussan): Generalize this @@ -896,11 +330,12 @@ void GraphLocalizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::Key } else { if (static_cast(factor_keys.size() - factor_key_indices_to_remove.size()) < params_.factor.smart_projection_adder.min_num_points) { - factor_it = graph_.erase(factor_it); + factor_it = graph_factors().erase(factor_it); continue; } else { const auto new_smart_factor = RemoveSmartFactorMeasurements( - *smart_factor, factor_key_indices_to_remove, params_.factor.smart_projection_adder, smart_projection_params_); + *smart_factor, factor_key_indices_to_remove, params_.factor.smart_projection_adder, + smart_projection_cumulative_factor_adder_->smart_projection_params()); *factor_it = new_smart_factor; continue; } @@ -908,128 +343,19 @@ void GraphLocalizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::Key } } -void GraphLocalizer::BufferFactors(const std::vector& factors_to_add_vec) { - for (const auto& factors_to_add : factors_to_add_vec) - buffered_factors_to_add_.emplace(factors_to_add.timestamp(), factors_to_add); -} - -void GraphLocalizer::RemoveOldBufferedFactors(const lc::Time oldest_allowed_timestamp) { - for (auto factors_to_add_it = buffered_factors_to_add_.begin(); - factors_to_add_it != buffered_factors_to_add_.end();) { - auto& factors_to_add = factors_to_add_it->second.Get(); - for (auto factor_to_add_it = factors_to_add.begin(); factor_to_add_it != factors_to_add.end();) { - bool removed_factor = false; - for (const auto& key_info : factor_to_add_it->key_infos) { - // Ignore static keys - if (key_info.is_static()) continue; - if (key_info.timestamp() < oldest_allowed_timestamp) { - LogDebug("RemoveOldBufferedFactors: Removing old factor from buffered factors."); - factor_to_add_it = factors_to_add.erase(factor_to_add_it); - removed_factor = true; - break; - } - } - if (!removed_factor) ++factor_to_add_it; - } - if (factors_to_add_it->second.Get().empty()) { - LogDebug("RemoveOldBufferedFactors: Removing old factors from buffered factors."); - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - } else { - ++factors_to_add_it; - } - } -} - -int GraphLocalizer::AddBufferedFactors() { - LogDebug("AddBufferedfactors: Adding buffered factors."); - LogDebug("AddBufferedFactors: Num buffered factors to add: " << buffered_factors_to_add_.size()); - - int num_added_factors = 0; - for (auto factors_to_add_it = buffered_factors_to_add_.begin(); - factors_to_add_it != buffered_factors_to_add_.end() && latest_imu_integrator_.LatestTime() && - factors_to_add_it->first <= *(latest_imu_integrator_.LatestTime());) { - auto& factors_to_add = factors_to_add_it->second; - for (auto& factor_to_add : factors_to_add.Get()) { - // Add combined nav states and connecting imu factors for each key in factor if necessary - // TODO(rsoussan): make this more efficient for factors with multiple keys with the same timestamp? - for (const auto& key_info : factor_to_add.key_infos) { - // Ignore static keys - if (key_info.is_static()) continue; - if (!AddOrSplitImuFactorIfNeeded(key_info.timestamp())) { - LogError("AddBufferedFactor: Failed to add or split imu factors necessary for adding factor."); - continue; - } - } - - if (!Rekey(factor_to_add)) { - LogError("AddBufferedMeasurements: Failed to rekey factor to add."); - continue; - } - } - - // Do graph action after adding necessary imu factors and nav states so these are available - if (!DoGraphAction(factors_to_add)) { - LogDebug("AddBufferedFactors: Failed to complete graph action."); - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - continue; - } - - for (auto& factor_to_add : factors_to_add.Get()) { - graph_.push_back(factor_to_add.factor); - ++num_added_factors; - } - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - } - - LogDebug("AddBufferedFactors: Added " << num_added_factors << " factors."); - return num_added_factors; -} - -bool GraphLocalizer::DoGraphAction(FactorsToAdd& factors_to_add) { - switch (factors_to_add.graph_action()) { - case GraphAction::kNone: - return true; - case GraphAction::kDeleteExistingSmartFactors: - LogDebug("DoGraphAction: Deleting smart factors."); - DeleteFactors(); - // TODO(rsoussan): rename this graph action to handle smart factors - if (params_.factor.smart_projection_adder.splitting) SplitSmartFactorsIfNeeded(factors_to_add); - return true; - case GraphAction::kTriangulateNewPoint: - return TriangulateNewPoint(factors_to_add); - case GraphAction::kLocProjectionNoiseScaling: - return LocProjectionNoiseScaling(factors_to_add); - case GraphAction::kARTagProjectionNoiseScaling: - return ARProjectionNoiseScaling(factors_to_add); - } - - // Shouldn't occur - return true; -} - -bool GraphLocalizer::Rekey(FactorToAdd& factor_to_add) { - gtsam::KeyVector new_keys; - const auto& old_keys = factor_to_add.factor->keys(); - for (int i = 0; i < static_cast(factor_to_add.key_infos.size()); ++i) { - const auto& key_info = factor_to_add.key_infos[i]; - if (key_info.is_static()) { - // Don't change static keys. Assumes static key currently in factor is correct - new_keys.emplace_back(old_keys[i]); - } else { - const auto new_key = graph_values_->GetKey(key_info.key_creator_function(), key_info.timestamp()); - if (!new_key) { - LogError("ReKey: Failed to find new key for timestamp."); - return false; - } - new_keys.emplace_back(*new_key); - } - } - factor_to_add.factor->keys() = new_keys; - return true; +bool GraphLocalizer::ValidGraph() const { + // If graph consists of only priors and imu factors, consider it invalid and don't optimize. + // Make sure smart factors are valid before including them. + const int num_valid_non_imu_measurement_factors = NumOFFactors(true) + + go::NumFactors(graph_factors()) + + go::NumFactors>(graph_factors()) + + go::NumFactors(graph_factors()) + + go::NumFactors>(graph_factors()); + return num_valid_non_imu_measurement_factors > 0; } -bool GraphLocalizer::ReadyToAddMeasurement(const localization_common::Time timestamp) const { - const auto latest_time = latest_imu_integrator_.LatestTime(); +bool GraphLocalizer::ReadyToAddFactors(const localization_common::Time timestamp) const { + const auto latest_time = latest_imu_integrator_->LatestTime(); if (!latest_time) { LogError("ReadyToAddMeasurement: Failed to get latet imu time."); return false; @@ -1039,17 +365,17 @@ bool GraphLocalizer::ReadyToAddMeasurement(const localization_common::Time times } bool GraphLocalizer::MeasurementRecentEnough(const lc::Time timestamp) const { - if (!latest_imu_integrator_.OldestTime()) { + if (!GraphOptimizer::MeasurementRecentEnough(timestamp)) return false; + if (!latest_imu_integrator_->OldestTime()) { LogDebug("MeasurementRecentEnough: Waiting until imu measurements have been received."); return false; } - if (timestamp < graph_values_->OldestTimestamp()) return false; - if (timestamp < latest_imu_integrator_.OldestTime()) return false; + if (timestamp < latest_imu_integrator_->OldestTime()) return false; return true; } void GraphLocalizer::PrintFactorDebugInfo() const { - for (const auto& factor : graph_) { + for (const auto& factor : graph_factors()) { const auto smart_factor = dynamic_cast(factor.get()); if (smart_factor) { smart_factor->print(); @@ -1071,34 +397,37 @@ void GraphLocalizer::PrintFactorDebugInfo() const { } void GraphLocalizer::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { - latest_imu_integrator_.SetFanSpeedMode(fan_speed_mode); + latest_imu_integrator_->SetFanSpeedMode(fan_speed_mode); } -const lm::FanSpeedMode GraphLocalizer::fan_speed_mode() const { return latest_imu_integrator_.fan_speed_mode(); } +const lm::FanSpeedMode GraphLocalizer::fan_speed_mode() const { return latest_imu_integrator_->fan_speed_mode(); } const GraphLocalizerParams& GraphLocalizer::params() const { return params_; } -int GraphLocalizer::NumFeatures() const { return graph_values_->NumFeatures(); } +int GraphLocalizer::NumFeatures() const { return feature_point_node_updater_->NumFeatures(); } // TODO(rsoussan): fix this call to happen before of factors are removed! int GraphLocalizer::NumOFFactors(const bool check_valid) const { - if (params_.factor.smart_projection_adder.enabled) return NumSmartFactors(check_valid); + if (params_.factor.smart_projection_adder.enabled) + return NumSmartFactors(graph_factors(), combined_nav_state_node_updater_->graph_values().values(), check_valid); if (params_.factor.projection_adder.enabled) return NumProjectionFactors(check_valid); return 0; } int GraphLocalizer::NumProjectionFactors(const bool check_valid) const { int num_factors = 0; - for (const auto& factor : graph_) { + for (const auto& factor : graph_factors()) { const auto projection_factor = dynamic_cast(factor.get()); if (projection_factor) { if (check_valid) { - const auto world_t_point = graph_values_->at(projection_factor->key2()); + const auto world_t_point = + feature_point_node_updater_->feature_point_graph_values().at(projection_factor->key2()); if (!world_t_point) { LogError("NumProjectionFactors: Failed to get point."); continue; } - const auto world_T_body = graph_values_->at(projection_factor->key1()); + const auto world_T_body = + combined_nav_state_node_updater_->graph_values().at(projection_factor->key1()); if (!world_T_body) { LogError("NumProjectionFactors: Failed to get pose."); continue; @@ -1118,34 +447,10 @@ int GraphLocalizer::NumProjectionFactors(const bool check_valid) const { return num_factors; } -int GraphLocalizer::NumSmartFactors(const bool check_valid) const { - int num_of_factors = 0; - for (const auto& factor : graph_) { - const auto smart_factor = dynamic_cast(factor.get()); - if (smart_factor) { - if (check_valid) { - if (smart_factor->isValid()) ++num_of_factors; - } else { - ++num_of_factors; - } - } - } - return num_of_factors; -} - -const GraphValues& GraphLocalizer::graph_values() const { return *graph_values_; } - -const gtsam::NonlinearFactorGraph& GraphLocalizer::factor_graph() const { return graph_; } - -void GraphLocalizer::SaveGraphDotFile(const std::string& output_path) const { - std::ofstream of(output_path.c_str()); - graph_.saveGraph(of, graph_values_->values()); +const GraphLocalizerStats& GraphLocalizer::graph_localizer_stats() const { + return *(static_cast(graph_stats())); } -const GraphStats& GraphLocalizer::graph_stats() const { return graph_stats_; } - -void GraphLocalizer::LogOnDestruction(const bool log_on_destruction) { log_on_destruction_ = log_on_destruction; } - bool GraphLocalizer::standstill() const { // If uninitialized, return not at standstill // TODO(rsoussan): Is this the appropriate behavior? @@ -1153,105 +458,15 @@ bool GraphLocalizer::standstill() const { return *standstill_; } -bool GraphLocalizer::Update() { - LogDebug("Update: Updating."); - graph_stats_.update_timer_.Start(); - - graph_stats_.add_buffered_factors_timer_.Start(); - BufferCumulativeFactors(); - const int num_added_factors = AddBufferedFactors(); - graph_stats_.add_buffered_factors_timer_.Stop(); - if (num_added_factors <= 0) { - LogDebug("Update: No factors added."); - return false; - } - - // Only get marginals and slide window if optimization has already occured - // TODO(rsoussan): Make cleaner way to check for this - if (last_latest_time_) { - graph_stats_.marginals_timer_.Start(); - // Calculate marginals for covariances - try { - marginals_ = gtsam::Marginals(graph_, graph_values_->values(), marginals_factorization_); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); - marginals_ = boost::none; - } catch (...) { - log(params_.fatal_failures, "Update: Computing marginals failed."); - marginals_ = boost::none; - } - graph_stats_.marginals_timer_.Stop(); - - graph_stats_.slide_window_timer_.Start(); - if (!SlideWindow(marginals_, *last_latest_time_)) { - LogError("Update: Failed to slide window."); - return false; - } - graph_stats_.slide_window_timer_.Stop(); - } - - // TODO(rsoussan): Is ordering required? if so clean these calls open and unify with marginalization - // TODO(rsoussan): Remove this now that marginalization occurs before optimization? - if (params_.add_marginal_factors) { - // Add graph ordering to place keys that will be marginalized in first group - const auto new_oldest_time = graph_values_->SlideWindowNewOldestTime(); - if (new_oldest_time) { - const auto old_keys = graph_values_->OldKeys(*new_oldest_time); - const auto ordering = gtsam::Ordering::ColamdConstrainedFirst(graph_, old_keys); - levenberg_marquardt_params_.setOrdering(ordering); - } else { - levenberg_marquardt_params_.orderingType = gtsam::Ordering::COLAMD; - } - } - - // Optimize - gtsam::LevenbergMarquardtOptimizer optimizer(graph_, graph_values_->values(), levenberg_marquardt_params_); - - graph_stats_.optimization_timer_.Start(); - // TODO(rsoussan): Indicate if failure occurs in state msg, perhaps using confidence value for localizer - try { - graph_values_->UpdateValues(optimizer.optimize()); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Indeterminant linear system error during optimization, keeping old values."); - } catch (...) { - log(params_.fatal_failures, "Update: Graph optimization failed, keeping old values."); - } - graph_stats_.optimization_timer_.Stop(); - - // Calculate marginals after the first optimization iteration so covariances - // can be used for first loc msg - // TODO(rsoussan): Clean this up - if (!last_latest_time_) { - graph_stats_.marginals_timer_.Start(); - // Calculate marginals for covariances - try { - marginals_ = gtsam::Marginals(graph_, graph_values_->values(), marginals_factorization_); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); - marginals_ = boost::none; - } catch (...) { - log(params_.fatal_failures, "Update: Computing marginals failed."); - marginals_ = boost::none; - } - graph_stats_.marginals_timer_.Stop(); - } - - last_latest_time_ = graph_values_->LatestTimestamp(); - graph_stats_.iterations_averager_.Update(optimizer.iterations()); - graph_stats_.UpdateStats(*this); - graph_stats_.UpdateErrors(*this); - - if (params_.print_factor_info) PrintFactorDebugInfo(); - +bool GraphLocalizer::DoPostOptimizeActions() { // Update imu integrator bias - const auto latest_bias = graph_values_->LatestBias(); + const auto latest_bias = combined_nav_state_node_updater_->graph_values().LatestBias(); if (!latest_bias) { LogError("Update: Failed to get latest bias."); return false; } - latest_imu_integrator_.ResetPimIntegrationAndSetBias(latest_bias->first); - graph_stats_.update_timer_.Stop(); + latest_imu_integrator_->ResetPimIntegrationAndSetBias(latest_bias->first); return true; } } // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_nodelet.cc b/localization/graph_localizer/src/graph_localizer_nodelet.cc index 57114cd97a..f3072f40dd 100644 --- a/localization/graph_localizer/src/graph_localizer_nodelet.cc +++ b/localization/graph_localizer/src/graph_localizer_nodelet.cc @@ -32,8 +32,7 @@ namespace graph_localizer { namespace lc = localization_common; namespace mc = msg_conversions; -GraphLocalizerNodelet::GraphLocalizerNodelet() - : ff_util::FreeFlyerNodelet(NODE_GRAPH_LOC, true), platform_name_(GetPlatform()) { +GraphLocalizerNodelet::GraphLocalizerNodelet() : ff_util::FreeFlyerNodelet(NODE_GRAPH_LOC, true) { private_nh_.setCallbackQueue(&private_queue_); heartbeat_.node = GetName(); heartbeat_.nodelet_manager = ros::this_node::getName(); @@ -47,6 +46,10 @@ GraphLocalizerNodelet::GraphLocalizerNodelet() } void GraphLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + // Setup the platform name + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); + ff_common::InitFreeFlyerApplication(getMyArgv()); SubscribeAndAdvertise(nh); Run(); diff --git a/localization/graph_localizer/src/graph_localizer_stats.cc b/localization/graph_localizer/src/graph_localizer_stats.cc new file mode 100644 index 0000000000..6798f54d92 --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer_stats.cc @@ -0,0 +1,153 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +GraphLocalizerStats::GraphLocalizerStats() { + AddStatsAverager(num_states_averager_); + AddStatsAverager(duration_averager_); + AddStatsAverager(num_marginal_factors_averager_); + AddStatsAverager(num_factors_averager_); + // AddStatsAverager(num_features_averager_); + AddStatsAverager(num_optical_flow_factors_averager_); + AddStatsAverager(num_loc_pose_factors_averager_); + AddStatsAverager(num_loc_proj_factors_averager_); + AddStatsAverager(num_imu_factors_averager_); + AddStatsAverager(num_rotation_factors_averager_); + AddStatsAverager(num_standstill_between_factors_averager_); + AddStatsAverager(num_vel_prior_factors_averager_); + + AddErrorAverager(of_error_averager_); + AddErrorAverager(loc_proj_error_averager_); + AddErrorAverager(loc_pose_error_averager_); + AddErrorAverager(imu_error_averager_); + AddErrorAverager(rotation_error_averager_); + AddErrorAverager(standstill_between_error_averager_); + AddErrorAverager(pose_prior_error_averager_); + AddErrorAverager(velocity_prior_error_averager_); + AddErrorAverager(bias_prior_error_averager_); +} + +void GraphLocalizerStats::SetCombinedNavStateGraphValues( + std::shared_ptr combined_nav_state_graph_values) { + combined_nav_state_graph_values_ = std::move(combined_nav_state_graph_values); +} + +void GraphLocalizerStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) { + using Calibration = gtsam::Cal3_S2; + using Camera = gtsam::PinholePose; + using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor; + using ProjectionFactor = gtsam::GenericProjectionFactor; + + double total_error = 0; + double optical_flow_factor_error = 0; + double loc_proj_error = 0; + double loc_pose_error = 0; + double imu_factor_error = 0; + double rotation_factor_error = 0; + double standstill_between_factor_error = 0; + double pose_prior_error = 0; + double velocity_prior_error = 0; + double bias_prior_error = 0; + for (const auto& factor : graph_factors) { + const double error = factor->error(combined_nav_state_graph_values_->values()); + total_error += error; + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + optical_flow_factor_error += error; + } + const auto projection_factor = dynamic_cast(factor.get()); + if (projection_factor) { + optical_flow_factor_error += error; + } + const auto imu_factor = dynamic_cast(factor.get()); + if (imu_factor) { + imu_factor_error += error; + } + const auto loc_factor = dynamic_cast*>(factor.get()); + if (loc_factor) { + loc_proj_error += error; + } + const auto loc_pose_factor = dynamic_cast(factor.get()); + if (loc_pose_factor) { + loc_pose_error += error; + } + const auto rotation_factor = dynamic_cast(factor.get()); + if (rotation_factor) { + rotation_factor_error += error; + } + const auto standstill_between_factor = dynamic_cast*>(factor.get()); + if (standstill_between_factor) { + standstill_between_factor_error += error; + } + + // Prior Factors + const auto pose_prior_factor = dynamic_cast*>(factor.get()); + if (pose_prior_factor && !loc_pose_factor) { + pose_prior_error += error; + } + const auto velocity_prior_factor = dynamic_cast*>(factor.get()); + if (velocity_prior_factor) { + velocity_prior_error += error; + } + const auto bias_prior_factor = dynamic_cast*>(factor.get()); + if (bias_prior_factor) { + bias_prior_error += error; + } + } + total_error_averager_.Update(total_error); + of_error_averager_.Update(optical_flow_factor_error); + loc_proj_error_averager_.Update(loc_proj_error); + loc_pose_error_averager_.Update(loc_pose_error); + imu_error_averager_.Update(imu_factor_error); + rotation_error_averager_.Update(rotation_factor_error); + standstill_between_error_averager_.Update(standstill_between_factor_error); + pose_prior_error_averager_.Update(pose_prior_error); + velocity_prior_error_averager_.Update(velocity_prior_error); + bias_prior_error_averager_.Update(bias_prior_error); +} + +void GraphLocalizerStats::UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) { + num_states_averager_.Update(combined_nav_state_graph_values_->NumStates()); + duration_averager_.Update(combined_nav_state_graph_values_->Duration()); + num_marginal_factors_averager_.Update(go::NumFactors(graph_factors)); + num_factors_averager_.Update(graph_factors.size()); + num_optical_flow_factors_averager_.Update( + NumSmartFactors(graph_factors, combined_nav_state_graph_values_->values(), true)); + num_loc_pose_factors_averager_.Update(go::NumFactors(graph_factors)); + num_loc_proj_factors_averager_.Update(go::NumFactors>(graph_factors)); + num_imu_factors_averager_.Update(go::NumFactors(graph_factors)); + num_rotation_factors_averager_.Update(go::NumFactors(graph_factors)); + num_standstill_between_factors_averager_.Update(go::NumFactors>(graph_factors)); + num_vel_prior_factors_averager_.Update(go::NumFactors>(graph_factors)); +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_wrapper.cc b/localization/graph_localizer/src/graph_localizer_wrapper.cc index 32dcb3533a..09e8e3ffd9 100644 --- a/localization/graph_localizer/src/graph_localizer_wrapper.cc +++ b/localization/graph_localizer/src/graph_localizer_wrapper.cc @@ -86,8 +86,7 @@ void GraphLocalizerWrapper::Update() { void GraphLocalizerWrapper::OpticalFlowCallback(const ff_msgs::Feature2dArray& feature_array_msg) { feature_counts_.of = feature_array_msg.feature_array.size(); if (graph_localizer_) { - if (graph_localizer_->AddOpticalFlowMeasurement(lm::MakeFeaturePointsMeasurement(feature_array_msg))) { - } + graph_localizer_->AddOpticalFlowMeasurement(lm::MakeFeaturePointsMeasurement(feature_array_msg)); } } @@ -236,11 +235,16 @@ void GraphLocalizerWrapper::InitializeGraph() { graph_localizer_.reset(new graph_localizer::GraphLocalizer(graph_localizer_initializer_.params())); } -boost::optional GraphLocalizerWrapper::feature_tracks() const { +boost::optional GraphLocalizerWrapper::feature_tracks() const { if (!graph_localizer_) return boost::none; return graph_localizer_->feature_tracks(); } +boost::optional GraphLocalizerWrapper::graph_localizer() const { + if (!graph_localizer_) return boost::none; + return *graph_localizer_; +} + void GraphLocalizerWrapper::MarkWorldTDockForResettingIfNecessary() { if (estimate_world_T_dock_using_loc_) reset_world_T_dock_ = true; } @@ -304,7 +308,7 @@ boost::optional GraphLocalizerWrapper::LatestLocalizationSt GraphStateMsg(combined_nav_state_and_covariances->first, combined_nav_state_and_covariances->second, feature_counts_, graph_localizer_initializer_.EstimateBiases(), position_cov_log_det_lost_threshold_, orientation_cov_log_det_lost_threshold_, graph_localizer_->standstill(), - graph_localizer_->graph_stats(), graph_localizer_->fan_speed_mode()); + graph_localizer_->graph_localizer_stats(), graph_localizer_->fan_speed_mode()); feature_counts_.Reset(); return graph_state_msg; } @@ -321,12 +325,12 @@ void GraphLocalizerWrapper::SaveLocalizationGraphDotFile() const { if (graph_localizer_) graph_localizer_->SaveGraphDotFile(); } -boost::optional GraphLocalizerWrapper::graph_stats() const { +boost::optional GraphLocalizerWrapper::graph_localizer_stats() const { if (!graph_localizer_) { LogDebug("GraphStats: Failed to get graph stats."); return boost::none; } - return graph_localizer_->graph_stats(); + return graph_localizer_->graph_localizer_stats(); } bool GraphLocalizerWrapper::publish_localization_graph() const { return publish_localization_graph_; } diff --git a/localization/graph_localizer/src/graph_stats.cc b/localization/graph_localizer/src/graph_stats.cc deleted file mode 100644 index 5cbae24555..0000000000 --- a/localization/graph_localizer/src/graph_stats.cc +++ /dev/null @@ -1,169 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#include -#include -#include -#include -#include - -#include -#include - -namespace graph_localizer { -GraphStats::GraphStats() { - timers_.emplace_back(optimization_timer_); - timers_.emplace_back(update_timer_); - timers_.emplace_back(marginals_timer_); - timers_.emplace_back(slide_window_timer_); - timers_.emplace_back(add_buffered_factors_timer_); - timers_.emplace_back(log_error_timer_); - timers_.emplace_back(log_stats_timer_); - - stats_averagers_.emplace_back(iterations_averager_); - stats_averagers_.emplace_back(num_states_averager_); - stats_averagers_.emplace_back(duration_averager_); - stats_averagers_.emplace_back(num_optical_flow_factors_averager_); - stats_averagers_.emplace_back(num_loc_pose_factors_averager_); - stats_averagers_.emplace_back(num_loc_proj_factors_averager_); - stats_averagers_.emplace_back(num_imu_factors_averager_); - stats_averagers_.emplace_back(num_rotation_factors_averager_); - stats_averagers_.emplace_back(num_standstill_between_factors_averager_); - stats_averagers_.emplace_back(num_vel_prior_factors_averager_); - stats_averagers_.emplace_back(num_marginal_factors_averager_); - stats_averagers_.emplace_back(num_factors_averager_); - stats_averagers_.emplace_back(num_features_averager_); - - error_averagers_.emplace_back(total_error_averager_); - error_averagers_.emplace_back(of_error_averager_); - error_averagers_.emplace_back(loc_proj_error_averager_); - error_averagers_.emplace_back(loc_pose_error_averager_); - error_averagers_.emplace_back(imu_error_averager_); - error_averagers_.emplace_back(rotation_error_averager_); - error_averagers_.emplace_back(standstill_between_error_averager_); - error_averagers_.emplace_back(pose_prior_error_averager_); - error_averagers_.emplace_back(velocity_prior_error_averager_); - error_averagers_.emplace_back(bias_prior_error_averager_); -} - -void GraphStats::UpdateErrors(const GraphLocalizer& graph) { - log_error_timer_.Start(); - double total_error = 0; - double optical_flow_factor_error = 0; - double loc_proj_error = 0; - double loc_pose_error = 0; - double imu_factor_error = 0; - double rotation_factor_error = 0; - double standstill_between_factor_error = 0; - double pose_prior_error = 0; - double velocity_prior_error = 0; - double bias_prior_error = 0; - for (const auto& factor : graph.factor_graph()) { - const double error = factor->error(graph.graph_values().values()); - total_error += error; - const auto smart_factor = dynamic_cast(factor.get()); - if (smart_factor) { - optical_flow_factor_error += error; - } - const auto projection_factor = dynamic_cast(factor.get()); - if (projection_factor) { - optical_flow_factor_error += error; - } - const auto imu_factor = dynamic_cast(factor.get()); - if (imu_factor) { - imu_factor_error += error; - } - const auto loc_factor = dynamic_cast*>(factor.get()); - if (loc_factor) { - loc_proj_error += error; - } - const auto loc_pose_factor = dynamic_cast(factor.get()); - if (loc_pose_factor) { - loc_pose_error += error; - } - const auto rotation_factor = dynamic_cast(factor.get()); - if (rotation_factor) { - rotation_factor_error += error; - } - const auto standstill_between_factor = dynamic_cast*>(factor.get()); - if (standstill_between_factor) { - standstill_between_factor_error += error; - } - - // Prior Factors - const auto pose_prior_factor = dynamic_cast*>(factor.get()); - if (pose_prior_factor && !loc_pose_factor) { - pose_prior_error += error; - } - const auto velocity_prior_factor = dynamic_cast*>(factor.get()); - if (velocity_prior_factor) { - velocity_prior_error += error; - } - const auto bias_prior_factor = dynamic_cast*>(factor.get()); - if (bias_prior_factor) { - bias_prior_error += error; - } - } - total_error_averager_.Update(total_error); - of_error_averager_.Update(optical_flow_factor_error); - loc_proj_error_averager_.Update(loc_proj_error); - loc_pose_error_averager_.Update(loc_pose_error); - imu_error_averager_.Update(imu_factor_error); - rotation_error_averager_.Update(rotation_factor_error); - standstill_between_error_averager_.Update(standstill_between_factor_error); - pose_prior_error_averager_.Update(pose_prior_error); - velocity_prior_error_averager_.Update(velocity_prior_error); - bias_prior_error_averager_.Update(bias_prior_error); - log_error_timer_.Stop(); -} - -void GraphStats::UpdateStats(const GraphLocalizer& graph) { - log_stats_timer_.Start(); - num_states_averager_.Update(graph.graph_values().NumStates()); - duration_averager_.Update(graph.graph_values().Duration()); - num_optical_flow_factors_averager_.Update(graph.NumOFFactors()); - num_loc_pose_factors_averager_.Update(graph.NumFactors()); - num_loc_proj_factors_averager_.Update(graph.NumFactors>()); - num_imu_factors_averager_.Update(graph.NumFactors()); - num_rotation_factors_averager_.Update(graph.NumFactors()); - num_standstill_between_factors_averager_.Update(graph.NumFactors>()); - num_vel_prior_factors_averager_.Update(graph.NumFactors>()); - num_marginal_factors_averager_.Update(graph.NumFactors()); - if (graph.params().factor.projection_adder.enabled) num_features_averager_.Update(graph.NumFeatures()); - num_factors_averager_.Update(graph.factor_graph().size()); - log_stats_timer_.Stop(); -} - -void GraphStats::Log() const { - Log(timers_); - Log(stats_averagers_); - Log(error_averagers_); -} - -void GraphStats::LogToFile(std::ofstream& ofstream) const { - LogToFile(timers_, ofstream); - LogToFile(stats_averagers_, ofstream); - LogToFile(error_averagers_, ofstream); -} - -void GraphStats::LogToCsv(std::ofstream& ofstream) const { - ofstream << "name,avg,min,max,stddev" << std::endl; - LogToCsv(timers_, ofstream); - LogToCsv(stats_averagers_, ofstream); - LogToCsv(error_averagers_, ofstream); -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/loc_factor_adder.cc b/localization/graph_localizer/src/loc_factor_adder.cc index b870024b44..0c2f34a574 100644 --- a/localization/graph_localizer/src/loc_factor_adder.cc +++ b/localization/graph_localizer/src/loc_factor_adder.cc @@ -16,7 +16,6 @@ * under the License. */ -#include #include #include #include @@ -26,12 +25,14 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; -LocFactorAdder::LocFactorAdder(const LocFactorAdderParams& params, const GraphAction projection_graph_action) - : LocFactorAdder::Base(params), projection_graph_action_(projection_graph_action) {} +LocFactorAdder::LocFactorAdder(const LocFactorAdderParams& params, + const go::GraphActionCompleterType graph_action_completer_type) + : LocFactorAdder::Base(params), graph_action_completer_type_(graph_action_completer_type) {} -std::vector LocFactorAdder::AddFactors( +std::vector LocFactorAdder::AddFactors( const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { if (matched_projections_measurement.matched_projections.empty()) { LogDebug("AddFactors: Empty measurement."); @@ -45,14 +46,14 @@ std::vector LocFactorAdder::AddFactors( const int num_landmarks = matched_projections_measurement.matched_projections.size(); num_landmarks_averager_.Update(num_landmarks); - std::vector factors_to_add; + std::vector factors_to_add; if (params().add_pose_priors) { double noise_scale = params().pose_noise_scale; if (params().scale_pose_noise_with_num_landmarks) { noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast(num_landmarks)), 2); } - FactorsToAdd pose_factors_to_add; + go::FactorsToAdd pose_factors_to_add; pose_factors_to_add.reserve(1); const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << params().prior_translation_stddev, params().prior_translation_stddev, params().prior_translation_stddev, @@ -63,7 +64,8 @@ std::vector LocFactorAdder::AddFactors( gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas)), params().huber_k); - const KeyInfo key_info(&sym::P, matched_projections_measurement.timestamp); + const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + matched_projections_measurement.timestamp); gtsam::LocPoseFactor::shared_ptr pose_prior_factor(new gtsam::LocPoseFactor( key_info.UninitializedKey(), matched_projections_measurement.global_T_cam * params().body_T_cam.inverse(), pose_noise)); @@ -79,10 +81,11 @@ std::vector LocFactorAdder::AddFactors( } int num_loc_projection_factors = 0; - FactorsToAdd projection_factors_to_add(projection_graph_action_); + go::FactorsToAdd projection_factors_to_add(type()); projection_factors_to_add.reserve(matched_projections_measurement.matched_projections.size()); for (const auto& matched_projection : matched_projections_measurement.matched_projections) { - const KeyInfo key_info(&sym::P, matched_projections_measurement.timestamp); + const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + matched_projections_measurement.timestamp); // TODO(rsoussan): Pass sigma insted of already constructed isotropic noise const gtsam::SharedIsotropic scaled_noise( gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params().cam_noise->sigma())); @@ -101,4 +104,6 @@ std::vector LocFactorAdder::AddFactors( } return factors_to_add; } + +go::GraphActionCompleterType LocFactorAdder::type() const { return graph_action_completer_type_; } } // namespace graph_localizer diff --git a/localization/graph_localizer/src/loc_graph_action_completer.cc b/localization/graph_localizer/src/loc_graph_action_completer.cc new file mode 100644 index 0000000000..1cc20e3394 --- /dev/null +++ b/localization/graph_localizer/src/loc_graph_action_completer.cc @@ -0,0 +1,106 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +LocGraphActionCompleter::LocGraphActionCompleter(const LocFactorAdderParams& params, + const go::GraphActionCompleterType graph_action_completer_type, + std::shared_ptr graph_values) + : params_(params), + graph_action_completer_type_(graph_action_completer_type), + graph_values_(std::move(graph_values)) {} + +go::GraphActionCompleterType LocGraphActionCompleter::type() const { return graph_action_completer_type_; } + +bool LocGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) { + auto& factors = factors_to_add.Get(); + boost::optional pose_key; + boost::optional world_T_cam; + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + auto& factor = factor_it->factor; + auto projection_factor = dynamic_cast*>(factor.get()); + if (!projection_factor) { + LogError("MapProjectionNoiseScaling: Failed to cast to projection factor."); + return false; + } + + // Save pose and pose key in case need to make loc prior + pose_key = projection_factor->key(); + world_T_cam = projection_factor->world_T_cam(); + + const auto world_T_body = graph_values_->at(projection_factor->key()); + if (!world_T_body) { + LogError("MapProjectionNoiseScaling: Failed to get pose."); + return false; + } + const auto error = (projection_factor->evaluateError(*world_T_body)).norm(); + const auto cheirality_error = projection_factor->cheiralityError(*world_T_body); + if (cheirality_error) { + factor_it = factors.erase(factor_it); + } else if (error > params_.max_inlier_weighted_projection_norm) { + factor_it = factors.erase(factor_it); + } else { + if (params_.weight_projections_with_distance) { + const gtsam::Point3 nav_cam_t_landmark = + (*world_T_body * *(projection_factor->body_P_sensor())).inverse() * projection_factor->landmark_point(); + const gtsam::SharedIsotropic scaled_noise( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + // Don't use robust cost here to more effectively correct a drift occurance + gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor(new gtsam::LocProjectionFactor<>( + projection_factor->measured(), projection_factor->landmark_point(), scaled_noise, projection_factor->key(), + projection_factor->calibration(), *(projection_factor->body_P_sensor()))); + factor_it->factor = loc_projection_factor; + } + ++factor_it; + } + } + // All factors have been removed due to errors, use loc pose prior instead + if (factors.empty() && params_.add_prior_if_projections_fail) { + if (!pose_key || !world_T_cam) { + LogError("MapProjectionNoiseScaling: Failed to get pose key and world_T_cam"); + return false; + } + const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << params_.prior_translation_stddev, + params_.prior_translation_stddev, params_.prior_translation_stddev, + params_.prior_quaternion_stddev, params_.prior_quaternion_stddev, + params_.prior_quaternion_stddev) + .finished()); + // TODO(rsoussan): enable scaling with num landmarks + const int noise_scale = 1; + const auto pose_noise = Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(noise_scale * pose_prior_noise_sigmas)), + params_.huber_k); + gtsam::LocPoseFactor::shared_ptr pose_prior_factor( + new gtsam::LocPoseFactor(*pose_key, *world_T_cam * params_.body_T_cam.inverse(), pose_noise)); + factors_to_add.push_back(go::FactorToAdd( + {go::KeyInfo(&sym::P, go::NodeUpdaterType::CombinedNavState, factors_to_add.timestamp())}, pose_prior_factor)); + } + return true; +} + +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/parameter_reader.cc b/localization/graph_localizer/src/parameter_reader.cc index 4e0adf1382..0754488250 100644 --- a/localization/graph_localizer/src/parameter_reader.cc +++ b/localization/graph_localizer/src/parameter_reader.cc @@ -18,11 +18,13 @@ #include #include +#include #include #include #include namespace graph_localizer { +namespace go = graph_optimizer; namespace ii = imu_integration; namespace lc = localization_common; namespace mc = msg_conversions; @@ -133,10 +135,13 @@ void LoadSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, params.max_num_factors = mc::LoadInt(config, "smart_projection_adder_max_num_factors"); params.min_num_points = mc::LoadInt(config, "smart_projection_adder_min_num_points"); params.max_num_points_per_factor = mc::LoadInt(config, "smart_projection_adder_max_num_points_per_factor"); + params.measurement_spacing = mc::LoadInt(config, "smart_projection_adder_measurement_spacing"); + params.feature_track_min_separation = mc::LoadDouble(config, "smart_projection_adder_feature_track_min_separation"); params.rotation_only_fallback = mc::LoadBool(config, "smart_projection_adder_rotation_only_fallback"); params.splitting = mc::LoadBool(config, "smart_projection_adder_splitting"); params.scale_noise_with_num_points = mc::LoadBool(config, "smart_projection_adder_scale_noise_with_num_points"); params.noise_scale = mc::LoadDouble(config, "smart_projection_adder_noise_scale"); + params.use_allowed_timestamps = mc::LoadBool(config, "smart_projection_adder_use_allowed_timestamps"); params.body_T_cam = lc::LoadTransform(config, "nav_cam_transform"); params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); params.cam_noise = @@ -157,25 +162,7 @@ void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, Stands void LoadFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params) { params.sliding_window_duration = mc::LoadDouble(config, "feature_tracker_sliding_window_duration"); -} - -void LoadStandstillFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params) { - params.sliding_window_duration = mc::LoadDouble(config, "standstill_feature_tracker_sliding_window_duration"); -} - -void LoadGraphValuesParams(config_reader::ConfigReader& config, GraphValuesParams& params) { - params.ideal_duration = mc::LoadDouble(config, "ideal_duration"); - params.min_num_states = mc::LoadInt(config, "min_num_states"); - params.max_num_states = mc::LoadInt(config, "max_num_states"); - params.min_num_factors_per_feature = mc::LoadInt(config, "min_num_factors_per_feature"); -} - -void LoadNoiseParams(config_reader::ConfigReader& config, NoiseParams& params) { - params.starting_prior_translation_stddev = mc::LoadDouble(config, "starting_prior_translation_stddev"); - params.starting_prior_quaternion_stddev = mc::LoadDouble(config, "starting_prior_quaternion_stddev"); - params.starting_prior_velocity_stddev = mc::LoadDouble(config, "starting_prior_velocity_stddev"); - params.starting_prior_accel_bias_stddev = mc::LoadDouble(config, "starting_prior_accel_bias_stddev"); - params.starting_prior_gyro_bias_stddev = mc::LoadDouble(config, "starting_prior_gyro_bias_stddev"); + params.smart_projection_adder_measurement_spacing = mc::LoadInt(config, "smart_projection_adder_measurement_spacing"); } void LoadSanityCheckerParams(config_reader::ConfigReader& config, SanityCheckerParams& params) { @@ -195,30 +182,45 @@ void LoadGraphInitializerParams(config_reader::ConfigReader& config, GraphInitia params.num_bias_estimation_measurements = mc::LoadInt(config, "num_bias_estimation_measurements"); } +void LoadCombinedNavStateNodeUpdaterParams(config_reader::ConfigReader& config, + CombinedNavStateNodeUpdaterParams& params) { + params.starting_prior_translation_stddev = mc::LoadDouble(config, "starting_prior_translation_stddev"); + params.starting_prior_quaternion_stddev = mc::LoadDouble(config, "starting_prior_quaternion_stddev"); + params.starting_prior_velocity_stddev = mc::LoadDouble(config, "starting_prior_velocity_stddev"); + params.starting_prior_accel_bias_stddev = mc::LoadDouble(config, "starting_prior_accel_bias_stddev"); + params.starting_prior_gyro_bias_stddev = mc::LoadDouble(config, "starting_prior_gyro_bias_stddev"); + params.huber_k = mc::LoadDouble(config, "huber_k"); + params.add_priors = mc::LoadBool(config, "add_priors"); + params.threshold_bias_uncertainty = mc::LoadBool(config, "threshold_bias_uncertainty"); + params.accel_bias_stddev_threshold = mc::LoadDouble(config, "accel_bias_stddev_threshold"); + params.gyro_bias_stddev_threshold = mc::LoadDouble(config, "gyro_bias_stddev_threshold"); + LoadCombinedNavStateGraphValuesParams(config, params.graph_values); +} + +void LoadCombinedNavStateGraphValuesParams(config_reader::ConfigReader& config, + CombinedNavStateGraphValuesParams& params) { + params.ideal_duration = mc::LoadDouble(config, "ideal_duration"); + params.min_num_states = mc::LoadInt(config, "min_num_states"); + params.max_num_states = mc::LoadInt(config, "max_num_states"); +} + +void LoadFeaturePointNodeUpdaterParams(config_reader::ConfigReader& config, FeaturePointNodeUpdaterParams& params) { + params.huber_k = mc::LoadDouble(config, "huber_k"); +} + void LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params) { LoadCalibrationParams(config, params.calibration); + LoadCombinedNavStateNodeUpdaterParams(config, params.combined_nav_state_node_updater); LoadGraphInitializerParams(config, params.graph_initializer); LoadFactorParams(config, params.factor); + LoadFeaturePointNodeUpdaterParams(config, params.feature_point_node_updater); LoadFeatureTrackerParams(config, params.feature_tracker); - LoadStandstillFeatureTrackerParams(config, params.standstill_feature_tracker); - LoadGraphValuesParams(config, params.graph_values); - LoadNoiseParams(config, params.noise); - params.verbose = mc::LoadBool(config, "verbose"); - params.fatal_failures = mc::LoadBool(config, "fatal_failures"); - params.print_factor_info = mc::LoadBool(config, "print_factor_info"); - params.use_ceres_params = mc::LoadBool(config, "use_ceres_params"); - params.max_iterations = mc::LoadInt(config, "max_iterations"); - params.marginals_factorization = mc::LoadString(config, "marginals_factorization"); - params.limit_imu_factor_spacing = mc::LoadBool(config, "limit_imu_factor_spacing"); - params.max_imu_factor_spacing = mc::LoadDouble(config, "max_imu_factor_spacing"); - params.add_priors = mc::LoadBool(config, "add_priors"); - params.add_marginal_factors = mc::LoadBool(config, "add_marginal_factors"); + go::LoadGraphOptimizerParams(config, params.graph_optimizer); params.huber_k = mc::LoadDouble(config, "huber_k"); params.max_standstill_feature_track_avg_distance_from_mean = mc::LoadDouble(config, "max_standstill_feature_track_avg_distance_from_mean"); params.standstill_min_num_points_per_track = mc::LoadInt(config, "standstill_min_num_points_per_track"); - params.log_rate = mc::LoadInt(config, "log_rate"); - params.optical_flow_measurement_spacing = mc::LoadInt(config, "optical_flow_measurement_spacing"); + params.standstill_feature_track_duration = mc::LoadDouble(config, "standstill_feature_track_duration"); params.estimate_world_T_dock_using_loc = mc::LoadBool(config, "estimate_world_T_dock_using_loc"); } diff --git a/localization/graph_localizer/src/projection_factor_adder.cc b/localization/graph_localizer/src/projection_factor_adder.cc index fa3ca76b66..7b47f25eeb 100644 --- a/localization/graph_localizer/src/projection_factor_adder.cc +++ b/localization/graph_localizer/src/projection_factor_adder.cc @@ -16,7 +16,6 @@ * under the License. */ -#include #include #include #include @@ -25,23 +24,26 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; ProjectionFactorAdder::ProjectionFactorAdder(const ProjectionFactorAdderParams& params, std::shared_ptr feature_tracker, - std::shared_ptr graph_values) - : ProjectionFactorAdder::Base(params), feature_tracker_(feature_tracker), graph_values_(graph_values) {} + std::shared_ptr feature_point_graph_values) + : ProjectionFactorAdder::Base(params), + feature_tracker_(feature_tracker), + feature_point_graph_values_(std::move(feature_point_graph_values)) {} -std::vector ProjectionFactorAdder::AddFactors( +std::vector ProjectionFactorAdder::AddFactors( const lm::FeaturePointsMeasurement& feature_points_measurement) { - std::vector factors_to_add_vec; + std::vector factors_to_add_vec; // Add projection factors for new measurements of already existing features - FactorsToAdd projection_factors_to_add; + go::FactorsToAdd projection_factors_to_add; for (const auto& feature_point : feature_points_measurement.feature_points) { - if (graph_values_->HasFeature(feature_point.feature_id)) { - const KeyInfo pose_key_info(&sym::P, feature_point.timestamp); - const KeyInfo static_point_key_info(&sym::F, feature_point.feature_id); - const auto point_key = graph_values_->FeatureKey(feature_point.feature_id); + if (feature_point_graph_values_->HasFeature(feature_point.feature_id)) { + const go::KeyInfo pose_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); + const go::KeyInfo static_point_key_info(&sym::F, go::NodeUpdaterType::FeaturePoint, feature_point.feature_id); + const auto point_key = feature_point_graph_values_->FeatureKey(feature_point.feature_id); if (!point_key) { LogError("AddFactors: Failed to get point key."); continue; @@ -61,17 +63,18 @@ std::vector ProjectionFactorAdder::AddFactors( // Add new feature tracks and measurements if possible int new_features = 0; for (const auto& feature_track_pair : feature_tracker_->feature_tracks()) { - const auto& feature_track = feature_track_pair.second; - if (static_cast(feature_track.points.size()) >= params().min_num_measurements_for_triangulation && - !graph_values_->HasFeature(feature_track.id) && - (new_features + graph_values_->NumFeatures()) < params().max_num_features) { + const auto& feature_track = *(feature_track_pair.second); + if (static_cast(feature_track.size()) >= params().min_num_measurements_for_triangulation && + !feature_point_graph_values_->HasFeature(feature_track.id()) && + (new_features + feature_point_graph_values_->NumFeatures()) < params().max_num_features) { // Create new factors to add for each feature track so the graph action can act on only that // feature track to triangulate a new point - FactorsToAdd projection_factors_with_new_point_to_add(GraphAction::kTriangulateNewPoint); - const auto point_key = graph_values_->CreateFeatureKey(); - for (const auto& feature_point : feature_track.points) { - const KeyInfo pose_key_info(&sym::P, feature_point.timestamp); - const KeyInfo static_point_key_info(&sym::F, feature_point.feature_id); + go::FactorsToAdd projection_factors_with_new_point_to_add(go::GraphActionCompleterType::ProjectionFactor); + const auto point_key = feature_point_graph_values_->CreateFeatureKey(); + for (const auto& feature_point_pair : feature_track.points()) { + const auto& feature_point = feature_point_pair.second; + const go::KeyInfo pose_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); + const go::KeyInfo static_point_key_info(&sym::F, go::NodeUpdaterType::FeaturePoint, feature_point.feature_id); const auto projection_factor = boost::make_shared( feature_point.image_point, Robust(params().cam_noise, params().huber_k), pose_key_info.UninitializedKey(), point_key, params().cam_intrinsics, params().body_T_cam); diff --git a/localization/graph_localizer/src/projection_graph_action_completer.cc b/localization/graph_localizer/src/projection_graph_action_completer.cc new file mode 100644 index 0000000000..20e5885fad --- /dev/null +++ b/localization/graph_localizer/src/projection_graph_action_completer.cc @@ -0,0 +1,102 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace sym = gtsam::symbol_shorthand; +ProjectionGraphActionCompleter::ProjectionGraphActionCompleter( + const ProjectionFactorAdderParams& params, std::shared_ptr graph_values, + std::shared_ptr feature_point_graph_values) + : params_(params), graph_values_(graph_values), feature_point_graph_values_(std::move(feature_point_graph_values)) { + projection_triangulation_params_.rankTolerance = 1e-9; + // TODO(rsoussan): why is Base necessary for these? + projection_triangulation_params_.enableEPI = params_.enable_EPI; + projection_triangulation_params_.landmarkDistanceThreshold = params_.landmark_distance_threshold; + projection_triangulation_params_.dynamicOutlierRejectionThreshold = params_.dynamic_outlier_rejection_threshold; +} + +go::GraphActionCompleterType ProjectionGraphActionCompleter::type() const { + return go::GraphActionCompleterType::ProjectionFactor; +} + +bool ProjectionGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, + gtsam::NonlinearFactorGraph& graph_factors) { + return TriangulateNewPoint(factors_to_add, graph_factors); +} + +bool ProjectionGraphActionCompleter::TriangulateNewPoint(go::FactorsToAdd& factors_to_add, + gtsam::NonlinearFactorGraph& graph_factors) { + gtsam::CameraSet camera_set; + Camera::MeasurementVector measurements; + gtsam::Key point_key; + for (const auto& factor_to_add : factors_to_add.Get()) { + const auto& factor = factor_to_add.factor; + const auto projection_factor = dynamic_cast(factor.get()); + if (!projection_factor) { + LogError("TriangulateNewPoint: Failed to cast to projection factor."); + return false; + } + const auto world_T_body = graph_values_->at(projection_factor->key1()); + if (!world_T_body) { + LogError("TriangulateNewPoint: Failed to get pose."); + return false; + } + + const gtsam::Pose3 world_T_camera = *world_T_body * params_.body_T_cam; + camera_set.emplace_back(world_T_camera, params_.cam_intrinsics); + measurements.emplace_back(projection_factor->measured()); + point_key = projection_factor->key2(); + } + gtsam::TriangulationResult world_t_triangulated_point; + // TODO(rsoussan): Gtsam shouldn't be throwing exceptions for this, but needed if enable_epi enabled. + // Is there a build setting that prevents cheirality from being thrown in this case? + try { + world_t_triangulated_point = gtsam::triangulateSafe(camera_set, measurements, projection_triangulation_params_); + } catch (...) { + LogDebug("TriangulateNewPoint: Exception occurred during triangulation"); + return false; + } + + if (!world_t_triangulated_point.valid()) { + LogDebug("TriangulateNewPoint: Failed to triangulate point"); + return false; + } + // TODO(rsoussan): clean this up + const auto feature_id = factors_to_add.Get().front().key_infos[1].id(); + feature_point_graph_values_->AddFeature(feature_id, *world_t_triangulated_point, point_key); + if (params_.add_point_priors) { + const gtsam::Vector3 point_prior_noise_sigmas((gtsam::Vector(3) << params_.point_prior_translation_stddev, + params_.point_prior_translation_stddev, + params_.point_prior_translation_stddev) + .finished()); + const auto point_noise = + Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(point_prior_noise_sigmas)), + params_.huber_k); + gtsam::PriorFactor point_prior_factor(point_key, *world_t_triangulated_point, point_noise); + graph_factors.push_back(point_prior_factor); + } + return true; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/rotation_factor_adder.cc b/localization/graph_localizer/src/rotation_factor_adder.cc index e3a95fb8b3..381a393ed7 100644 --- a/localization/graph_localizer/src/rotation_factor_adder.cc +++ b/localization/graph_localizer/src/rotation_factor_adder.cc @@ -28,22 +28,23 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; RotationFactorAdder::RotationFactorAdder(const RotationFactorAdderParams& params, std::shared_ptr feature_tracker) : RotationFactorAdder::Base(params), feature_tracker_(feature_tracker) {} -std::vector RotationFactorAdder::AddFactors(const lm::FeaturePointsMeasurement& measurement) { +std::vector RotationFactorAdder::AddFactors(const lm::FeaturePointsMeasurement& measurement) { std::vector points_1; std::vector points_2; double total_disparity = 0; for (const auto& feature_track_pair : feature_tracker_->feature_tracks()) { - const auto& feature_track = feature_track_pair.second; - if (feature_track.points.size() < 2) continue; + const auto& feature_track = *(feature_track_pair.second); + if (feature_track.size() < 2) continue; // Get points for most recent and second to most recent images - const auto& point_1 = feature_track.points[feature_track.points.size() - 2].image_point; - const auto& point_2 = feature_track.points.back().image_point; + const auto& point_1 = std::next(feature_track.points().crbegin())->second.image_point; + const auto& point_2 = feature_track.points().crbegin()->second.image_point; points_1.emplace_back(cv::Point2d(point_1.x(), point_1.y())); points_2.emplace_back(cv::Point2d(point_2.x(), point_2.y())); total_disparity += (point_1 - point_2).norm(); @@ -86,15 +87,16 @@ std::vector RotationFactorAdder::AddFactors(const lm::FeaturePoint const gtsam::Rot3 cam_1_R_cam_2(eigen_cam_2_R_cam_1.transpose()); const gtsam::Rot3 body_1_R_body_2 = body_R_cam * cam_1_R_cam_2 * body_R_cam.inverse(); // Create Rotation Factor - const KeyInfo pose_1_key_info(&sym::P, *(feature_tracker_->PreviousTimestamp())); - const KeyInfo pose_2_key_info(&sym::P, measurement.timestamp); + const go::KeyInfo pose_1_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + *(feature_tracker_->PreviousTimestamp())); + const go::KeyInfo pose_2_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, measurement.timestamp); const gtsam::Vector3 rotation_noise_sigmas( (gtsam::Vector(3) << params().rotation_stddev, params().rotation_stddev, params().rotation_stddev).finished()); const auto rotation_noise = Robust( gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(rotation_noise_sigmas)), params().huber_k); const auto rotation_factor = boost::make_shared( body_1_R_body_2, rotation_noise, pose_1_key_info.UninitializedKey(), pose_2_key_info.UninitializedKey()); - FactorsToAdd rotation_factors_to_add; + go::FactorsToAdd rotation_factors_to_add; rotation_factors_to_add.push_back({{pose_1_key_info, pose_2_key_info}, rotation_factor}); rotation_factors_to_add.SetTimestamp(pose_2_key_info.timestamp()); LogDebug("AddFactors: Added a rotation factor."); diff --git a/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc b/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc index a1051c39a5..8ab9fae35e 100644 --- a/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc +++ b/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc @@ -16,15 +16,16 @@ * under the License. */ -#include #include #include +#include #include #include #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; SmartProjectionCumulativeFactorAdder::SmartProjectionCumulativeFactorAdder( @@ -35,25 +36,63 @@ SmartProjectionCumulativeFactorAdder::SmartProjectionCumulativeFactorAdder( smart_projection_params_.setLandmarkDistanceThreshold(params.landmark_distance_threshold); smart_projection_params_.setDynamicOutlierRejectionThreshold(params.dynamic_outlier_rejection_threshold); smart_projection_params_.setRetriangulationThreshold(params.retriangulation_threshold); + if (params.rotation_only_fallback) smart_projection_params_.setDegeneracyMode(gtsam::DegeneracyMode::HANDLE_INFINITY); smart_projection_params_.setEnableEPI(params.enable_EPI); } -std::vector SmartProjectionCumulativeFactorAdder::AddFactors() { - // Add smart factor for each valid feature track - FactorsToAdd smart_factors_to_add(GraphAction::kDeleteExistingSmartFactors); - int num_added_smart_factors = 0; - for (const auto& feature_track : feature_tracker_->feature_tracks()) { - const double average_distance_from_mean = AverageDistanceFromMean(feature_track.second.points); - if (ValidPointSet(feature_track.second.points, average_distance_from_mean, params().min_avg_distance_from_mean, +void SmartProjectionCumulativeFactorAdder::AddFactors( + const FeatureTrackLengthMap& feature_tracks, const int spacing, const double feature_track_min_separation, + go::FactorsToAdd& smart_factors_to_add, std::unordered_map& added_points) { + // Iterate in reverse order so longer feature tracks are prioritized + for (auto feature_track_it = feature_tracks.crbegin(); feature_track_it != feature_tracks.crend(); + ++feature_track_it) { + if (static_cast(smart_factors_to_add.size()) >= params().max_num_factors) break; + const auto& feature_track = *(feature_track_it->second); + const auto points = feature_track.LatestPoints(spacing); + // Skip already added tracks + if (added_points.count(points.front().feature_id) > 0) continue; + const double average_distance_from_mean = AverageDistanceFromMean(points); + if (ValidPointSet(points.size(), average_distance_from_mean, params().min_avg_distance_from_mean, params().min_num_points) && - num_added_smart_factors < params().max_num_factors) { - AddSmartFactor(feature_track.second, smart_factors_to_add); - ++num_added_smart_factors; + !TooClose(added_points, points.front(), feature_track_min_separation)) { + AddSmartFactor(points, smart_factors_to_add); + // Use latest point + added_points.emplace(points.front().feature_id, points.front()); } } +} +std::vector SmartProjectionCumulativeFactorAdder::AddFactors() { + // Add smart factor for each valid feature track + go::FactorsToAdd smart_factors_to_add(go::GraphActionCompleterType::SmartFactor); + if (params().use_allowed_timestamps) { + for (const auto& feature_track : feature_tracker_->feature_tracks()) { + const auto points = feature_track.second->AllowedPoints(feature_tracker_->smart_factor_timestamp_allow_list()); + const double average_distance_from_mean = AverageDistanceFromMean(points); + if (ValidPointSet(points.size(), average_distance_from_mean, params().min_avg_distance_from_mean, + params().min_num_points) && + static_cast(smart_factors_to_add.size()) < params().max_num_factors) { + AddSmartFactor(points, smart_factors_to_add); + } + } + } else { + const auto& feature_tracks = feature_tracker_->feature_tracks_length_ordered(); + const auto& longest_feature_track = feature_tracker_->LongestFeatureTrack(); + if (!longest_feature_track) { + LogDebug("AddFactors: Failed to get longest feature track."); + return {}; + } + const int spacing = longest_feature_track->MaxSpacing(params().max_num_points_per_factor); + + std::unordered_map added_points; + AddFactors(feature_tracks, spacing, params().feature_track_min_separation, smart_factors_to_add, added_points); + if (static_cast(smart_factors_to_add.size()) < params().max_num_factors) { + // Zero min separation so any valid feature track is added as a fallback to try to add up to max_num_factors + AddFactors(feature_tracks, spacing, 0, smart_factors_to_add, added_points); + } + } if (smart_factors_to_add.empty()) return {}; - const auto latest_timestamp = feature_tracker_->latest_timestamp(); + const auto latest_timestamp = feature_tracker_->LatestTimestamp(); if (!latest_timestamp) { LogError("AddFactors: Failed to get latest timestamp."); return {}; @@ -63,29 +102,44 @@ std::vector SmartProjectionCumulativeFactorAdder::AddFactors() { return {smart_factors_to_add}; } -void SmartProjectionCumulativeFactorAdder::AddSmartFactor(const FeatureTrack& feature_track, - FactorsToAdd& smart_factors_to_add) const { +void SmartProjectionCumulativeFactorAdder::AddSmartFactor(const std::vector& feature_track_points, + go::FactorsToAdd& smart_factors_to_add) const { SharedRobustSmartFactor smart_factor; - const int num_feature_track_points = feature_track.points.size(); - const auto noise = params().scale_noise_with_num_points - ? gtsam::noiseModel::Isotropic::Sigma( - 2, params().noise_scale * num_feature_track_points * params().cam_noise->sigma()) - : params().cam_noise; + const int num_feature_track_points = feature_track_points.size(); + const double noise_scale = + params().scale_noise_with_num_points ? params().noise_scale * num_feature_track_points : params().noise_scale; + const auto noise = gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params().cam_noise->sigma()); smart_factor = boost::make_shared(noise, params().cam_intrinsics, params().body_T_cam, smart_projection_params_, params().rotation_only_fallback, params().robust, params().huber_k); - KeyInfos key_infos; - key_infos.reserve(feature_track.points.size()); + go::KeyInfos key_infos; + key_infos.reserve(feature_track_points.size()); // Gtsam requires unique key indices for each key, even though these will be replaced later int uninitialized_key_index = 0; - for (int i = 0; i < feature_track.points.size(); ++i) { - const auto& feature_point = feature_track.points[i]; + for (int i = 0; i < static_cast(feature_track_points.size()); ++i) { + const auto& feature_point = feature_track_points[i]; if (i >= params().max_num_points_per_factor) break; - const KeyInfo key_info(&sym::P, feature_point.timestamp); + const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); key_infos.emplace_back(key_info); smart_factor->add(Camera::Measurement(feature_point.image_point), key_info.MakeKey(uninitialized_key_index++)); } smart_factors_to_add.push_back({key_infos, smart_factor}); } + +bool SmartProjectionCumulativeFactorAdder::TooClose( + const std::unordered_map& added_points, const lm::FeaturePoint& point, + const double feature_track_min_separation) const { + for (const auto& added_point_pair : added_points) { + const auto& added_point = added_point_pair.second; + if (((added_point.image_point - point.image_point).norm()) < feature_track_min_separation) { + return true; + } + } + return false; +} + +const gtsam::SmartProjectionParams& SmartProjectionCumulativeFactorAdder::smart_projection_params() const { + return smart_projection_params_; +} } // namespace graph_localizer diff --git a/localization/graph_localizer/src/smart_projection_graph_action_completer.cc b/localization/graph_localizer/src/smart_projection_graph_action_completer.cc new file mode 100644 index 0000000000..d6be435eb5 --- /dev/null +++ b/localization/graph_localizer/src/smart_projection_graph_action_completer.cc @@ -0,0 +1,82 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +SmartProjectionGraphActionCompleter::SmartProjectionGraphActionCompleter( + const SmartProjectionFactorAdderParams& params, std::shared_ptr graph_values) + : params_(params), graph_values_(std::move(graph_values)) { + smart_projection_params_.verboseCheirality = params_.verbose_cheirality; + smart_projection_params_.setRankTolerance(1e-9); + smart_projection_params_.setLandmarkDistanceThreshold(params_.landmark_distance_threshold); + smart_projection_params_.setDynamicOutlierRejectionThreshold(params_.dynamic_outlier_rejection_threshold); + smart_projection_params_.setRetriangulationThreshold(params_.retriangulation_threshold); + smart_projection_params_.setEnableEPI(params_.enable_EPI); +} + +go::GraphActionCompleterType SmartProjectionGraphActionCompleter::type() const { + return go::GraphActionCompleterType::SmartFactor; +} + +bool SmartProjectionGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, + gtsam::NonlinearFactorGraph& graph_factors) { + go::DeleteFactors(graph_factors); + if (params_.splitting) SplitSmartFactorsIfNeeded(*graph_values_, factors_to_add); + return true; +} + +// TODO(rsoussan): Clean this function up (duplicate code), address other todo's +void SmartProjectionGraphActionCompleter::SplitSmartFactorsIfNeeded(const CombinedNavStateGraphValues& graph_values, + go::FactorsToAdd& factors_to_add) { + for (auto& factor_to_add : factors_to_add.Get()) { + auto smart_factor = dynamic_cast(factor_to_add.factor.get()); + if (!smart_factor) continue; + // Can't remove measurements if there are only 2 or fewer + if (smart_factor->measured().size() <= 2) continue; + const auto point = smart_factor->triangulateSafe(smart_factor->cameras(graph_values.values())); + if (point.valid()) continue; + { + const auto fixed_smart_factor = + FixSmartFactorByRemovingIndividualMeasurements(params_, *smart_factor, smart_projection_params_, graph_values); + if (fixed_smart_factor) { + factor_to_add.factor = *fixed_smart_factor; + continue; + } + } + { + const auto fixed_smart_factor = + FixSmartFactorByRemovingMeasurementSequence(params_, *smart_factor, smart_projection_params_, graph_values); + if (fixed_smart_factor) { + factor_to_add.factor = *fixed_smart_factor; + continue; + } + } + LogDebug("SplitSmartFactorsIfNeeded: Failed to fix smart factor"); + } +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/standstill_factor_adder.cc b/localization/graph_localizer/src/standstill_factor_adder.cc index 9f837d74f9..e8636bfc9f 100644 --- a/localization/graph_localizer/src/standstill_factor_adder.cc +++ b/localization/graph_localizer/src/standstill_factor_adder.cc @@ -25,17 +25,18 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; StandstillFactorAdder::StandstillFactorAdder(const StandstillFactorAdderParams& params, std::shared_ptr feature_tracker) : StandstillFactorAdder::Base(params), feature_tracker_(feature_tracker) {} -std::vector StandstillFactorAdder::AddFactors( +std::vector StandstillFactorAdder::AddFactors( const lm::FeaturePointsMeasurement& feature_points_measurement) { - std::vector factors_to_add; + std::vector factors_to_add; if (params().add_velocity_prior) { - FactorsToAdd standstill_prior_factors_to_add; + go::FactorsToAdd standstill_prior_factors_to_add; const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params().prior_velocity_stddev, params().prior_velocity_stddev, params().prior_velocity_stddev) .finished()); @@ -43,7 +44,8 @@ std::vector StandstillFactorAdder::AddFactors( Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), params().huber_k); - const KeyInfo velocity_key_info(&sym::V, feature_points_measurement.timestamp); + const go::KeyInfo velocity_key_info(&sym::V, go::NodeUpdaterType::CombinedNavState, + feature_points_measurement.timestamp); gtsam::PriorFactor::shared_ptr velocity_prior_factor(new gtsam::PriorFactor( velocity_key_info.UninitializedKey(), gtsam::Velocity3::Zero(), velocity_noise)); standstill_prior_factors_to_add.push_back({{velocity_key_info}, velocity_prior_factor}); @@ -54,7 +56,7 @@ std::vector StandstillFactorAdder::AddFactors( if (params().add_pose_between_factor) { const auto previous_timestamp = feature_tracker_->PreviousTimestamp(); if (previous_timestamp) { - FactorsToAdd pose_between_factors_to_add; + go::FactorsToAdd pose_between_factors_to_add; const gtsam::Vector6 pose_between_noise_sigmas( (gtsam::Vector(6) << params().pose_between_factor_rotation_stddev, params().pose_between_factor_rotation_stddev, params().pose_between_factor_rotation_stddev, params().pose_between_factor_translation_stddev, @@ -63,8 +65,9 @@ std::vector StandstillFactorAdder::AddFactors( const auto pose_between_noise = Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_between_noise_sigmas)), params().huber_k); - const KeyInfo previous_between_key_info(&sym::P, *previous_timestamp); - const KeyInfo current_between_key_info(&sym::P, feature_points_measurement.timestamp); + const go::KeyInfo previous_between_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, *previous_timestamp); + const go::KeyInfo current_between_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + feature_points_measurement.timestamp); gtsam::BetweenFactor::shared_ptr pose_between_factor(new gtsam::BetweenFactor( previous_between_key_info.UninitializedKey(), current_between_key_info.UninitializedKey(), gtsam::Pose3::identity(), pose_between_noise)); diff --git a/localization/graph_localizer/src/utilities.cc b/localization/graph_localizer/src/utilities.cc index 7345c2d6e9..ee63399b49 100644 --- a/localization/graph_localizer/src/utilities.cc +++ b/localization/graph_localizer/src/utilities.cc @@ -16,6 +16,7 @@ * under the License. */ +#include #include #include #include @@ -27,18 +28,19 @@ #include namespace graph_localizer { +namespace go = graph_optimizer; namespace ii = imu_integration; namespace lc = localization_common; namespace lm = localization_measurements; namespace mc = msg_conversions; -bool ValidPointSet(const std::deque& points, const double average_distance_from_mean, +bool ValidPointSet(const int num_points, const double average_distance_from_mean, const double min_avg_distance_from_mean, const int min_num_points) { - if (static_cast(points.size()) < min_num_points) return false; + if (num_points < min_num_points) return false; return (average_distance_from_mean >= min_avg_distance_from_mean); } -double AverageDistanceFromMean(const std::deque& points) { +double AverageDistanceFromMean(const std::vector& points) { // Calculate mean point and avg distance from mean Eigen::Vector2d sum_of_points = Eigen::Vector2d::Zero(); for (const auto& point : points) { @@ -63,7 +65,7 @@ ff_msgs::GraphState GraphStateMsg(const lc::CombinedNavState& combined_nav_state const lc::CombinedNavStateCovariances& covariances, const FeatureCounts& detected_feature_counts, const bool estimating_bias, const double position_log_det_threshold, const double orientation_log_det_threshold, - const bool standstill, const GraphStats& graph_stats, + const bool standstill, const GraphLocalizerStats& graph_stats, const lm::FanSpeedMode fan_speed_mode) { ff_msgs::GraphState loc_msg; @@ -78,7 +80,8 @@ ff_msgs::GraphState GraphStateMsg(const lc::CombinedNavState& combined_nav_state lc::CombinedNavStateCovariancesToMsg(covariances, loc_msg); // Set Confidence - loc_msg.confidence = covariances.PoseConfidence(position_log_det_threshold, orientation_log_det_threshold); + // Controller can't handle a confidence other than 0. TODO(rsoussan): switch back when this is fixed. + loc_msg.confidence = 0; // covariances.PoseConfidence(position_log_det_threshold, orientation_log_det_threshold); // Set Graph Feature Counts/Information loc_msg.num_detected_of_features = detected_feature_counts.of; @@ -138,8 +141,8 @@ gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& nois } boost::optional FixSmartFactorByRemovingIndividualMeasurements( - const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values) { + const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values) { // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor const auto original_measurements = smart_factor.measured(); const auto original_keys = smart_factor.keys(); @@ -155,10 +158,8 @@ boost::optional FixSmartFactorByRemovingIndividualMeasu keys_to_add.emplace_back(original_keys[i]); } auto new_smart_factor = boost::make_shared( - params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, - params.factor.smart_projection_adder.body_T_cam, smart_projection_params, - params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, - params.factor.smart_projection_adder.huber_k); + params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, + params.rotation_only_fallback, params.robust, params.huber_k); new_smart_factor->add(measurements_to_add, keys_to_add); const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); if (new_point.valid()) { @@ -171,8 +172,8 @@ boost::optional FixSmartFactorByRemovingIndividualMeasu } boost::optional FixSmartFactorByRemovingMeasurementSequence( - const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values) { + const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values) { constexpr int min_num_measurements = 2; // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor const auto original_measurements = smart_factor.measured(); @@ -187,10 +188,8 @@ boost::optional FixSmartFactorByRemovingMeasurementSequ keys_to_add.emplace_back(original_keys[i]); } auto new_smart_factor = boost::make_shared( - params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, - params.factor.smart_projection_adder.body_T_cam, smart_projection_params, - params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, - params.factor.smart_projection_adder.huber_k); + params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, + params.rotation_only_fallback, params.robust, params.huber_k); new_smart_factor->add(measurements_to_add, keys_to_add); const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); if (new_point.valid()) { @@ -216,10 +215,8 @@ boost::optional FixSmartFactorByRemovingMeasurementSequ keys_to_add.emplace_back(original_keys[i]); } auto new_smart_factor = boost::make_shared( - params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, - params.factor.smart_projection_adder.body_T_cam, smart_projection_params, - params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, - params.factor.smart_projection_adder.huber_k); + params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, + params.rotation_only_fallback, params.robust, params.huber_k); new_smart_factor->add(measurements_to_add, keys_to_add); const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); if (new_point.valid()) { @@ -253,4 +250,20 @@ SharedRobustSmartFactor RemoveSmartFactorMeasurements(const RobustSmartFactor& s } return new_smart_factor; } + +int NumSmartFactors(const gtsam::NonlinearFactorGraph& graph_factors, const gtsam::Values& values, + const bool check_valid) { + int num_of_factors = 0; + for (const auto& factor : graph_factors) { + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + if (check_valid) { + if (smart_factor->valid(values)) ++num_of_factors; + } else { + ++num_of_factors; + } + } + } + return num_of_factors; +} } // namespace graph_localizer diff --git a/localization/graph_optimizer/CMakeLists.txt b/localization/graph_optimizer/CMakeLists.txt new file mode 100644 index 0000000000..cbc0a62a79 --- /dev/null +++ b/localization/graph_optimizer/CMakeLists.txt @@ -0,0 +1,32 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +project(graph_optimizer) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} config_reader localization_common localization_measurements msg_conversions + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + DEPENDS gtsam +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} gtsam config_reader localization_common localization_measurements msg_conversions + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS +) diff --git a/localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h b/localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h similarity index 80% rename from localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h rename to localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h index 2a594eb1ca..5599a5803c 100644 --- a/localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h +++ b/localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h @@ -16,14 +16,14 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ +#ifndef GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ +#define GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ -#include +#include #include -namespace graph_localizer { +namespace graph_optimizer { template class CumulativeFactorAdder { public: @@ -38,6 +38,6 @@ class CumulativeFactorAdder { private: PARAMS params_; }; -} // namespace graph_localizer +} // namespace graph_optimizer -#endif // GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ +#endif // GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_adder.h b/localization/graph_optimizer/include/graph_optimizer/factor_adder.h similarity index 83% rename from localization/graph_localizer/include/graph_localizer/factor_adder.h rename to localization/graph_optimizer/include/graph_optimizer/factor_adder.h index b8e8312ae8..e8530dac37 100644 --- a/localization/graph_localizer/include/graph_localizer/factor_adder.h +++ b/localization/graph_optimizer/include/graph_optimizer/factor_adder.h @@ -16,14 +16,14 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_FACTOR_ADDER_H_ +#ifndef GRAPH_OPTIMIZER_FACTOR_ADDER_H_ +#define GRAPH_OPTIMIZER_FACTOR_ADDER_H_ -#include +#include #include -namespace graph_localizer { +namespace graph_optimizer { template class FactorAdder { public: @@ -38,6 +38,6 @@ class FactorAdder { private: PARAMS params_; }; -} // namespace graph_localizer +} // namespace graph_optimizer -#endif // GRAPH_LOCALIZER_FACTOR_ADDER_H_ +#endif // GRAPH_OPTIMIZER_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_adder_params.h b/localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h similarity index 80% rename from localization/graph_localizer/include/graph_localizer/factor_adder_params.h rename to localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h index 92f4845406..73b14436d0 100644 --- a/localization/graph_localizer/include/graph_localizer/factor_adder_params.h +++ b/localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h @@ -16,14 +16,14 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ +#ifndef GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ +#define GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ -namespace graph_localizer { +namespace graph_optimizer { struct FactorAdderParams { bool enabled; double huber_k; }; -} // namespace graph_localizer +} // namespace graph_optimizer -#endif // GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ +#endif // GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_to_add.h b/localization/graph_optimizer/include/graph_optimizer/factor_to_add.h similarity index 70% rename from localization/graph_localizer/include/graph_localizer/factor_to_add.h rename to localization/graph_optimizer/include/graph_optimizer/factor_to_add.h index 0c5547a339..d522ade8ee 100644 --- a/localization/graph_localizer/include/graph_localizer/factor_to_add.h +++ b/localization/graph_optimizer/include/graph_optimizer/factor_to_add.h @@ -16,18 +16,18 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ -#define GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ +#ifndef GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ +#define GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ -#include -#include +#include +#include #include #include #include -namespace graph_localizer { +namespace graph_optimizer { struct FactorToAdd { FactorToAdd(const KeyInfos& key_infos, boost::shared_ptr factor) : factor(factor), key_infos(key_infos) {} @@ -39,10 +39,13 @@ struct FactorToAdd { class FactorsToAdd { public: FactorsToAdd(const localization_common::Time timestamp, const std::vector& factors_to_add, - const GraphAction graph_action = GraphAction::kNone) - : timestamp_(timestamp), factors_to_add_(factors_to_add), graph_action_(graph_action) {} + const GraphActionCompleterType graph_action_completer_type = GraphActionCompleterType::None) + : timestamp_(timestamp), + factors_to_add_(factors_to_add), + graph_action_completer_type_(graph_action_completer_type) {} - explicit FactorsToAdd(const GraphAction graph_action = GraphAction::kNone) : graph_action_(graph_action) {} + explicit FactorsToAdd(const GraphActionCompleterType graph_action_completer_type = GraphActionCompleterType::None) + : graph_action_completer_type_(graph_action_completer_type) {} void reserve(const int size) { factors_to_add_.reserve(size); } size_t size() const { return factors_to_add_.size(); } @@ -54,14 +57,14 @@ class FactorsToAdd { localization_common::Time timestamp() const { return timestamp_; } const std::vector& Get() const { return factors_to_add_; } std::vector& Get() { return factors_to_add_; } - GraphAction graph_action() const { return graph_action_; } + GraphActionCompleterType graph_action_completer_type() const { return graph_action_completer_type_; } private: // Timestamp used to sort factors when adding to graph. localization_common::Time timestamp_; std::vector factors_to_add_; - GraphAction graph_action_; + GraphActionCompleterType graph_action_completer_type_; }; -} // namespace graph_localizer +} // namespace graph_optimizer -#endif // GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ +#endif // GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h b/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h new file mode 100644 index 0000000000..9fe1d65fff --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ +#define GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ + +#include +#include + +#include + +namespace graph_optimizer { +class GraphActionCompleter { + public: + virtual ~GraphActionCompleter() {} + virtual bool DoAction(FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) = 0; + virtual GraphActionCompleterType type() const = 0; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h b/localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h new file mode 100644 index 0000000000..9fe744bc25 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ +#define GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ + +namespace graph_optimizer { +// TODO(rsoussan): Generalize this better +enum class GraphActionCompleterType { + ARTagLocProjectionFactor, + LocProjectionFactor, + None, + ProjectionFactor, + SmartFactor +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h new file mode 100644 index 0000000000..0d7cb52ffc --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h @@ -0,0 +1,161 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ +#define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace graph_optimizer { +class GraphOptimizer { + public: + GraphOptimizer(const GraphOptimizerParams& params, + std::unique_ptr graph_stats = std::unique_ptr(new GraphStats())); + // Default constructor/destructor for serialization only + GraphOptimizer() {} + ~GraphOptimizer(); + + // Registers graph action completer. Called after adding buffered factors and updating nodes + void AddGraphActionCompleter(std::shared_ptr graph_action_completer); + // Registers node updater. Called when adding buffered factors + void AddNodeUpdater(std::shared_ptr node_updater); + // Adds factors to buffered list which is sorted by time + void BufferFactors(const std::vector& factors_to_add_vec); + // Adds buffered factors and optimizes graph. Calls DoPostOptimizeActions afterwards + bool Update(); + // Checks whether a measurement is too old to be inserted into graph + virtual bool MeasurementRecentEnough(const localization_common::Time timestamp) const; + const gtsam::NonlinearFactorGraph& graph_factors() const; + gtsam::NonlinearFactorGraph& graph_factors(); + void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; + const GraphOptimizerParams& params() const; + void LogOnDestruction(const bool log_on_destruction); + const GraphStats* const graph_stats() const; + GraphStats* graph_stats(); + const boost::optional& marginals() const; + std::shared_ptr values(); + + private: + gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors, + const gtsam::KeyVector& old_keys, + const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const; + + // Removes Keys and Values outside of sliding window. + // Removes any factors depending on removed values + // Optionally adds marginalized factors encapsulating linearized error of removed factors + // Optionally adds priors using marginalized covariances for new oldest states + bool SlideWindow(const boost::optional& marginals, + const localization_common::Time last_latest_time); + + boost::optional SlideWindowNewOldestTime() const; + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time) const; + + std::pair OldKeysAndFactors( + const localization_common::Time oldest_allowed_time); + + // Called after SlideWindow + virtual void DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, + const boost::optional& marginals); + + // void UpdatePointPriors(const gtsam::Marginals& marginals); + + // Creates factors for cumulative factor adders and adds them to buffered list + virtual void BufferCumulativeFactors(); + + // Removes old measurements from cumulative factors that do not fit in sliding window so cumulative factors are still + // valid after the SlideWindow call + virtual void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys); + + // Optional validity check for graph before optimizing + virtual bool ValidGraph() const; + + // Adds buffered factors to graph_factors for future optimization + // Only adds factors that pass ReadyToAddFactors(timestamp) check + int AddBufferedFactors(); + + // Calls Update for each registered NodeUpdater to create required nodes while inserting new factors + bool UpdateNodes(const KeyInfo& key_info); + + // Calls DoAction for each registered GraphActionCompleter after inserting new factors + bool DoGraphAction(FactorsToAdd& factors_to_add); + + boost::optional GetKey(KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const; + + // Updates keys in factors after new nodes are created/updated as required by registered NodeUpdater + bool Rekey(FactorToAdd& factor_to_add); + + // Checks whether the graph is ready for insertion of a factor with timestamp + virtual bool ReadyToAddFactors(const localization_common::Time timestamp) const; + + boost::optional OldestTimestamp() const; + + boost::optional LatestTimestamp() const; + + // Removes buffered factors that are too old for insertion into graph + void RemoveOldBufferedFactors(const localization_common::Time oldest_allowed_timestamp); + + virtual void PrintFactorDebugInfo() const; + + // Called after optimizing graph + virtual bool DoPostOptimizeActions(); + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(graph_); + ar& BOOST_SERIALIZATION_NVP(values_); + } + + std::unique_ptr graph_stats_; + std::shared_ptr values_; + bool log_on_destruction_; + GraphOptimizerParams params_; + gtsam::LevenbergMarquardtParams levenberg_marquardt_params_; + gtsam::NonlinearFactorGraph graph_; + boost::optional marginals_; + std::multimap buffered_factors_to_add_; + + std::vector> node_updaters_; + std::vector> graph_action_completers_; + gtsam::Marginals::Factorization marginals_factorization_; + boost::optional last_latest_time_; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h new file mode 100644 index 0000000000..05851ba95b --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ +#define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ + +#include + +namespace graph_optimizer { +struct GraphOptimizerParams { + bool verbose; + bool fatal_failures; + bool print_factor_info; + bool use_ceres_params; + int max_iterations; + std::string marginals_factorization; + bool add_marginal_factors; + double huber_k; + int log_rate; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_stats.h b/localization/graph_optimizer/include/graph_optimizer/graph_stats.h new file mode 100644 index 0000000000..a22bc6d5f0 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_stats.h @@ -0,0 +1,76 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GRAPH_OPTIMIZER_GRAPH_STATS_H_ +#define GRAPH_OPTIMIZER_GRAPH_STATS_H_ + +#include +#include + +#include + +#include + +namespace graph_optimizer { +class GraphStats { + public: + GraphStats(); + void AddStatsAverager(localization_common::Averager& stats_averager); + void AddErrorAverager(localization_common::Averager& error_averager); + virtual void UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors); + virtual void UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors); + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + + std::vector> timers_; + std::vector> stats_averagers_; + std::vector> error_averagers_; + + // Timers + localization_common::Timer optimization_timer_ = localization_common::Timer("Optimization"); + localization_common::Timer update_timer_ = localization_common::Timer("Update"); + localization_common::Timer marginals_timer_ = localization_common::Timer("Marginals"); + localization_common::Timer slide_window_timer_ = localization_common::Timer("Slide Window"); + localization_common::Timer add_buffered_factors_timer_ = localization_common::Timer("Add Buffered Factors"); + localization_common::Timer log_error_timer_ = localization_common::Timer("Log Error"); + localization_common::Timer log_stats_timer_ = localization_common::Timer("Log Stats"); + + // Graph Stats Averagers + localization_common::Averager iterations_averager_ = localization_common::Averager("Iterations"); + // Factor Error Averagers + localization_common::Averager total_error_averager_ = localization_common::Averager("Total Factor Error"); + + private: + template + void LogToFile(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToFile(ofstream); + } + + template + void LogToCsv(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToCsv(ofstream); + } + + template + void Log(const std::vector>& loggers) const { + for (const auto& logger : loggers) logger.get().Log(); + } +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_STATS_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_values.h b/localization/graph_optimizer/include/graph_optimizer/graph_values.h new file mode 100644 index 0000000000..d65895fb23 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/graph_values.h @@ -0,0 +1,83 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_GRAPH_VALUES_H_ +#define GRAPH_OPTIMIZER_GRAPH_VALUES_H_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace graph_optimizer { +namespace sym = gtsam::symbol_shorthand; +class GraphValues { + public: + GraphValues(std::shared_ptr values = std::shared_ptr(new gtsam::Values())); + + // Returns the oldest time that will be in graph values once the window is slid using params + virtual boost::optional SlideWindowNewOldestTime() const = 0; + + virtual gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const = 0; + + virtual boost::optional GetKey(KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const = 0; + + virtual boost::optional OldestTimestamp() const = 0; + + virtual boost::optional LatestTimestamp() const = 0; + + template + boost::optional at(const gtsam::Key& key) const { + if (!values_->exists(key)) { + LogError("at: Key not present in values."); + return boost::none; + } + + return values_->at(key); + } + + const gtsam::Values& values() const; + + gtsam::Values& values(); + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(values_); + } + + std::shared_ptr values_; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/key_info.h b/localization/graph_optimizer/include/graph_optimizer/key_info.h similarity index 67% rename from localization/graph_localizer/include/graph_localizer/key_info.h rename to localization/graph_optimizer/include/graph_optimizer/key_info.h index a341e9b86f..e7e967da82 100644 --- a/localization/graph_localizer/include/graph_localizer/key_info.h +++ b/localization/graph_optimizer/include/graph_optimizer/key_info.h @@ -16,9 +16,10 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_KEY_INFO_H_ -#define GRAPH_LOCALIZER_KEY_INFO_H_ +#ifndef GRAPH_OPTIMIZER_KEY_INFO_H_ +#define GRAPH_OPTIMIZER_KEY_INFO_H_ +#include #include #include @@ -26,16 +27,25 @@ #include #include -namespace graph_localizer { +namespace graph_optimizer { + using KeyCreatorFunction = std::function; class KeyInfo { public: - KeyInfo(KeyCreatorFunction key_creator_function, const localization_common::Time timestamp) - : key_creator_function_(key_creator_function), timestamp_(timestamp), static_(false) {} + KeyInfo(KeyCreatorFunction key_creator_function, const NodeUpdaterType node_updater_type, + const localization_common::Time timestamp) + : key_creator_function_(key_creator_function), + node_updater_type_(node_updater_type), + timestamp_(timestamp), + static_(false) {} // Use for static keys // TODO(rsoussan): Clean up this interface, use inheritance instead? - KeyInfo(KeyCreatorFunction key_creator_function, const int id) - : key_creator_function_(key_creator_function), timestamp_(0), id_(id), static_(true) {} + KeyInfo(KeyCreatorFunction key_creator_function, const NodeUpdaterType node_updater_type, const int id) + : key_creator_function_(key_creator_function), + node_updater_type_(node_updater_type), + timestamp_(0), + id_(id), + static_(true) {} gtsam::Key UninitializedKey() const { return key_creator_function_(0); } gtsam::Key MakeKey(const std::uint64_t key_index) const { return key_creator_function_(key_index); } static gtsam::Key UninitializedKey(KeyCreatorFunction key_creator_function) { return key_creator_function(0); } @@ -44,9 +54,11 @@ class KeyInfo { bool is_static() const { return static_; } // TODO(rsoussan): This is only available for static keys, clean this up int id() const { return id_; } + NodeUpdaterType node_updater_type() const { return node_updater_type_; } private: KeyCreatorFunction key_creator_function_; + NodeUpdaterType node_updater_type_; localization_common::Time timestamp_; int id_; // Static (not changing with time) key @@ -54,6 +66,6 @@ class KeyInfo { }; using KeyInfos = std::vector; -} // namespace graph_localizer +} // namespace graph_optimizer -#endif // GRAPH_LOCALIZER_KEY_INFO_H_ +#endif // GRAPH_OPTIMIZER_KEY_INFO_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater.h b/localization/graph_optimizer/include/graph_optimizer/node_updater.h new file mode 100644 index 0000000000..4932cd8853 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/node_updater.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_H_ +#define GRAPH_OPTIMIZER_NODE_UPDATER_H_ + +#include +#include +#include + +#include +#include + +namespace graph_optimizer { +class NodeUpdater { + public: + virtual ~NodeUpdater() {} + + virtual bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) = 0; + + virtual bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) = 0; + + // Returns the oldest time that will be in graph values once the window is slid using params + virtual boost::optional SlideWindowNewOldestTime() const = 0; + + // TODO(rsoussan): consolidate these with graph values base? + virtual gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const = 0; + + virtual boost::optional GetKey(KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const = 0; + + virtual boost::optional OldestTimestamp() const = 0; + + virtual boost::optional LatestTimestamp() const = 0; + + virtual NodeUpdaterType type() const = 0; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_NODE_UPDATER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater_type.h b/localization/graph_optimizer/include/graph_optimizer/node_updater_type.h new file mode 100644 index 0000000000..17e8c88365 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/node_updater_type.h @@ -0,0 +1,27 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ +#define GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ + +namespace graph_optimizer { +// TODO(rsoussan): Generalize this better +enum class NodeUpdaterType { CombinedNavState, FeaturePoint }; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h b/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h new file mode 100644 index 0000000000..769b246376 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ +#define GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ + +#include + +#include + +namespace graph_optimizer { +template +class NodeUpdaterWithPriors : public NodeUpdater { + public: + virtual ~NodeUpdaterWithPriors() {} + + virtual void AddInitialValuesAndPriors(const NodeType& node, const NoiseType& noise, + gtsam::NonlinearFactorGraph& graph) = 0; + + virtual void AddPriors(const NodeType& node, const NoiseType& noise, gtsam::NonlinearFactorGraph& factors) = 0; +}; +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/parameter_reader.h b/localization/graph_optimizer/include/graph_optimizer/parameter_reader.h new file mode 100644 index 0000000000..f719c28a6b --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/parameter_reader.h @@ -0,0 +1,28 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef GRAPH_OPTIMIZER_PARAMETER_READER_H_ +#define GRAPH_OPTIMIZER_PARAMETER_READER_H_ + +#include +#include + +namespace graph_optimizer { +void LoadGraphOptimizerParams(config_reader::ConfigReader& config, GraphOptimizerParams& params); +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_PARAMETER_READER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/serialization.h b/localization/graph_optimizer/include/graph_optimizer/serialization.h new file mode 100644 index 0000000000..2d54656b13 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/serialization.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_SERIALIZATION_H_ +#define GRAPH_OPTIMIZER_SERIALIZATION_H_ + +#include + +#include + +namespace graph_optimizer { +std::string SerializeBinary(const GraphLocalizer& graph_optimizer); +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_SERIALIZATION_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/utilities.h b/localization/graph_optimizer/include/graph_optimizer/utilities.h new file mode 100644 index 0000000000..8617cb4305 --- /dev/null +++ b/localization/graph_optimizer/include/graph_optimizer/utilities.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_OPTIMIZER_UTILITIES_H_ +#define GRAPH_OPTIMIZER_UTILITIES_H_ + +#include + +#include + +namespace graph_optimizer { +gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& graph); + +template +void DeleteFactors(gtsam::NonlinearFactorGraph& graph) { + int num_removed_factors = 0; + for (auto factor_it = graph.begin(); factor_it != graph.end();) { + if (dynamic_cast(factor_it->get())) { + factor_it = graph.erase(factor_it); + ++num_removed_factors; + continue; + } + ++factor_it; + } + LogDebug("DeleteFactors: Num removed factors: " << num_removed_factors); +} + +template +int NumFactors(const gtsam::NonlinearFactorGraph& graph) { + int num_factors = 0; + for (const auto& factor : graph) { + if (dynamic_cast(factor.get())) { + ++num_factors; + } + } + return num_factors; +} + +} // namespace graph_optimizer + +#endif // GRAPH_OPTIMIZER_UTILITIES_H_ diff --git a/localization/graph_optimizer/package.xml b/localization/graph_optimizer/package.xml new file mode 100644 index 0000000000..81b9009955 --- /dev/null +++ b/localization/graph_optimizer/package.xml @@ -0,0 +1,17 @@ + + graph_optimizer + 1.0.0 + + The graph optimizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + diff --git a/localization/graph_optimizer/readme.md b/localization/graph_optimizer/readme.md new file mode 100644 index 0000000000..bc6b54d717 --- /dev/null +++ b/localization/graph_optimizer/readme.md @@ -0,0 +1 @@ +\page graphoptimizer Graph Optimizer diff --git a/localization/graph_optimizer/src/graph_optimizer.cc b/localization/graph_optimizer/src/graph_optimizer.cc new file mode 100644 index 0000000000..110639f42a --- /dev/null +++ b/localization/graph_optimizer/src/graph_optimizer.cc @@ -0,0 +1,473 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +namespace { +// TODO(rsoussan): Is this necessary? Just use DFATAL and compile with debug? +// Avoid having to compile with DEBUG to toggle between fatal and non-fatal failures +void log(const bool fatal_failure, const std::string& description) { + if (fatal_failure) { + LogFatal(description); + } else { + LogError(description); + } +} +} // namespace + +namespace graph_optimizer { +namespace lc = localization_common; + +GraphOptimizer::GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr graph_stats) + : graph_stats_(std::move(graph_stats)), values_(new gtsam::Values()), log_on_destruction_(true), params_(params) { + // Initialize lm params + if (params_.verbose) { + levenberg_marquardt_params_.verbosityLM = gtsam::LevenbergMarquardtParams::VerbosityLM::TRYDELTA; + levenberg_marquardt_params_.verbosity = gtsam::NonlinearOptimizerParams::Verbosity::LINEAR; + } + if (params_.use_ceres_params) { + gtsam::LevenbergMarquardtParams::SetCeresDefaults(&levenberg_marquardt_params_); + } + + levenberg_marquardt_params_.maxIterations = params_.max_iterations; + + if (params_.marginals_factorization == "qr") { + marginals_factorization_ = gtsam::Marginals::Factorization::QR; + } else if (params_.marginals_factorization == "cholesky") { + marginals_factorization_ = gtsam::Marginals::Factorization::CHOLESKY; + } else { + LogError("GraphOptimizer: No marginals factorization entered, defaulting to qr."); + marginals_factorization_ = gtsam::Marginals::Factorization::QR; + } +} + +GraphOptimizer::~GraphOptimizer() { + if (log_on_destruction_) graph_stats_->Log(); +} + +void GraphOptimizer::AddGraphActionCompleter(std::shared_ptr graph_action_completer) { + graph_action_completers_.emplace_back(std::move(graph_action_completer)); +} + +void GraphOptimizer::AddNodeUpdater(std::shared_ptr node_updater) { + node_updaters_.emplace_back(std::move(node_updater)); +} + +// Adapted from gtsam::BatchFixedLagSmoother +gtsam::NonlinearFactorGraph GraphOptimizer::MarginalFactors( + const gtsam::NonlinearFactorGraph& old_factors, const gtsam::KeyVector& old_keys, + const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const { + // Old keys not present in old factors. This shouldn't occur. + if (old_keys.size() == 0) { + LogDebug("MarginalFactors: No old keys provided."); + return old_factors; + } + + // Linearize Graph + const auto linearized_graph = old_factors.linearize(*values_); + const auto linear_marginal_factors = + *(linearized_graph->eliminatePartialMultifrontal(old_keys, eliminate_function).second); + return gtsam::LinearContainerFactor::ConvertLinearGraph(linear_marginal_factors, *values_); +} + +boost::optional GraphOptimizer::SlideWindowNewOldestTime() const { + boost::optional new_oldest_time; + for (const auto& node_updater : node_updaters_) { + const auto node_new_oldest_time = node_updater->SlideWindowNewOldestTime(); + if (node_new_oldest_time) { + new_oldest_time = new_oldest_time ? std::min(*node_new_oldest_time, *new_oldest_time) : *node_new_oldest_time; + } + } + + return new_oldest_time; +} + +gtsam::KeyVector GraphOptimizer::OldKeys(const localization_common::Time oldest_allowed_time) const { + gtsam::KeyVector all_old_keys; + for (const auto& node_updater : node_updaters_) { + const auto old_keys = node_updater->OldKeys(oldest_allowed_time, graph_); + all_old_keys.insert(all_old_keys.end(), old_keys.begin(), old_keys.end()); + } + return all_old_keys; +} + +std::pair GraphOptimizer::OldKeysAndFactors( + const lc::Time oldest_allowed_time) { + const auto old_keys = OldKeys(oldest_allowed_time); + // Since cumlative factors have many keys and shouldn't be marginalized, need to remove old measurements depending on + // old keys before marginalizing and sliding window + RemoveOldMeasurementsFromCumulativeFactors(old_keys); + const auto old_factors = RemoveOldFactors(old_keys, graph_); + return std::make_pair(old_keys, old_factors); +} + +bool GraphOptimizer::SlideWindow(const boost::optional& marginals, const lc::Time last_latest_time) { + const auto ideal_new_oldest_time = SlideWindowNewOldestTime(); + if (!ideal_new_oldest_time) { + LogDebug("SlideWindow: No states removed. "); + return true; + } + // Ensure that new oldest time isn't more recent than last latest time + // since then priors couldn't be added for the new oldest state + if (last_latest_time < *ideal_new_oldest_time) + LogError("SlideWindow: Ideal oldest time is more recent than last latest time."); + const auto new_oldest_time = std::min(last_latest_time, *ideal_new_oldest_time); + + const auto old_keys_and_factors = OldKeysAndFactors(new_oldest_time); + if (params_.add_marginal_factors) { + const auto marginal_factors = + MarginalFactors(old_keys_and_factors.second, old_keys_and_factors.first, gtsam::EliminateQR); + for (const auto& marginal_factor : marginal_factors) { + graph_.push_back(marginal_factor); + } + } + + for (auto& node_updater : node_updaters_) + node_updater->SlideWindow(new_oldest_time, marginals, old_keys_and_factors.first, params_.huber_k, graph_); + + RemoveOldBufferedFactors(new_oldest_time); + DoPostSlideWindowActions(new_oldest_time, marginals); + return true; +} + +void GraphOptimizer::DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, + const boost::optional& marginals) {} + +void GraphOptimizer::BufferCumulativeFactors() {} + +void GraphOptimizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) {} + +bool GraphOptimizer::ValidGraph() const { return true; } + +void GraphOptimizer::BufferFactors(const std::vector& factors_to_add_vec) { + for (const auto& factors_to_add : factors_to_add_vec) + buffered_factors_to_add_.emplace(factors_to_add.timestamp(), factors_to_add); +} + +void GraphOptimizer::RemoveOldBufferedFactors(const lc::Time oldest_allowed_timestamp) { + for (auto factors_to_add_it = buffered_factors_to_add_.begin(); + factors_to_add_it != buffered_factors_to_add_.end();) { + auto& factors_to_add = factors_to_add_it->second.Get(); + for (auto factor_to_add_it = factors_to_add.begin(); factor_to_add_it != factors_to_add.end();) { + bool removed_factor = false; + for (const auto& key_info : factor_to_add_it->key_infos) { + // Ignore static keys + if (key_info.is_static()) continue; + if (key_info.timestamp() < oldest_allowed_timestamp) { + LogDebug("RemoveOldBufferedFactors: Removing old factor from buffered factors."); + factor_to_add_it = factors_to_add.erase(factor_to_add_it); + removed_factor = true; + break; + } + } + if (!removed_factor) ++factor_to_add_it; + } + if (factors_to_add_it->second.Get().empty()) { + LogDebug("RemoveOldBufferedFactors: Removing old factors from buffered factors."); + factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); + } else { + ++factors_to_add_it; + } + } +} + +int GraphOptimizer::AddBufferedFactors() { + LogDebug("AddBufferedfactors: Adding buffered factors."); + LogDebug("AddBufferedFactors: Num buffered factors to add: " << buffered_factors_to_add_.size()); + + int num_added_factors = 0; + for (auto factors_to_add_it = buffered_factors_to_add_.begin(); + factors_to_add_it != buffered_factors_to_add_.end() && ReadyToAddFactors(factors_to_add_it->first);) { + auto& factors_to_add = factors_to_add_it->second; + for (auto& factor_to_add : factors_to_add.Get()) { + for (const auto& key_info : factor_to_add.key_infos) { + if (!UpdateNodes(key_info)) { + LogError("AddBufferedFactors: Failed to update nodes."); + } + } + + if (!Rekey(factor_to_add)) { + LogError("AddBufferedMeasurements: Failed to rekey factor to add."); + continue; + } + } + + if (!DoGraphAction(factors_to_add)) { + LogDebug("AddBufferedFactors: Failed to complete graph action."); + factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); + continue; + } + + for (auto& factor_to_add : factors_to_add.Get()) { + graph_.push_back(factor_to_add.factor); + ++num_added_factors; + } + factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); + } + + LogDebug("AddBufferedFactors: Added " << num_added_factors << " factors."); + return num_added_factors; +} + +bool GraphOptimizer::UpdateNodes(const KeyInfo& key_info) { + // Do nothing for static nodes + if (key_info.is_static()) return true; + for (auto& node_updater : node_updaters_) { + if (node_updater->type() == key_info.node_updater_type()) return node_updater->Update(key_info.timestamp(), graph_); + } + LogError("UpdateNodes: No node updater found for key info."); + return false; +} + +bool GraphOptimizer::DoGraphAction(FactorsToAdd& factors_to_add) { + if (factors_to_add.graph_action_completer_type() == GraphActionCompleterType::None) return true; + for (auto& graph_action_completer : graph_action_completers_) { + if (graph_action_completer->type() == factors_to_add.graph_action_completer_type()) + return graph_action_completer->DoAction(factors_to_add, graph_); + } + + LogError("DoGraphAction: No graph action completer found for factors to add."); + return false; +} + +boost::optional GraphOptimizer::GetKey(KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const { + // This avoids a bug in the compiler that produces a false warning that key may be uninitialized + boost::optional key = boost::make_optional(false, gtsam::Key()); + for (const auto& node_updater : node_updaters_) { + const auto node_key = node_updater->GetKey(key_creator_function, timestamp); + if (node_key) { + if (!key) { + key = node_key; + } else { + LogError("GetKey: Found key in multiple node updators."); + return boost::none; + } + } + } + return key; +} + +bool GraphOptimizer::Rekey(FactorToAdd& factor_to_add) { + gtsam::KeyVector new_keys; + const auto& old_keys = factor_to_add.factor->keys(); + for (int i = 0; i < static_cast(factor_to_add.key_infos.size()); ++i) { + const auto& key_info = factor_to_add.key_infos[i]; + if (key_info.is_static()) { + // Don't change static keys. Assumes static key currently in factor is correct + new_keys.emplace_back(old_keys[i]); + } else { + const auto new_key = GetKey(key_info.key_creator_function(), key_info.timestamp()); + if (!new_key) { + LogError("ReKey: Failed to find new key for timestamp."); + return false; + } + new_keys.emplace_back(*new_key); + } + } + factor_to_add.factor->keys() = new_keys; + return true; +} + +bool GraphOptimizer::ReadyToAddFactors(const localization_common::Time timestamp) const { return true; } + +boost::optional GraphOptimizer::OldestTimestamp() const { + boost::optional oldest_timestamp; + for (const auto& node_updater : node_updaters_) { + const auto node_oldest_timestamp = node_updater->OldestTimestamp(); + if (node_oldest_timestamp) { + oldest_timestamp = + oldest_timestamp ? std::min(*oldest_timestamp, *node_oldest_timestamp) : *node_oldest_timestamp; + } + } + return oldest_timestamp; +} + +boost::optional GraphOptimizer::LatestTimestamp() const { + boost::optional latest_timestamp; + for (const auto& node_updater : node_updaters_) { + const auto node_latest_timestamp = node_updater->LatestTimestamp(); + if (node_latest_timestamp) { + latest_timestamp = + latest_timestamp ? std::max(*latest_timestamp, *node_latest_timestamp) : *node_latest_timestamp; + } + } + return latest_timestamp; +} + +bool GraphOptimizer::MeasurementRecentEnough(const lc::Time timestamp) const { + const auto oldest_timestamp = OldestTimestamp(); + if (!oldest_timestamp) { + LogError("MeasurementRecentEnough: Failed to get oldest timestamp."); + return false; + } + if (timestamp < *oldest_timestamp) return false; + return true; +} + +void GraphOptimizer::PrintFactorDebugInfo() const {} + +const GraphOptimizerParams& GraphOptimizer::params() const { return params_; } + +const gtsam::NonlinearFactorGraph& GraphOptimizer::graph_factors() const { return graph_; } + +gtsam::NonlinearFactorGraph& GraphOptimizer::graph_factors() { return graph_; } + +const boost::optional& GraphOptimizer::marginals() const { return marginals_; } + +std::shared_ptr GraphOptimizer::values() { return values_; } + +void GraphOptimizer::SaveGraphDotFile(const std::string& output_path) const { + std::ofstream of(output_path.c_str()); + graph_.saveGraph(of, *values_); +} + +const GraphStats* const GraphOptimizer::graph_stats() const { return graph_stats_.get(); } + +GraphStats* GraphOptimizer::graph_stats() { return graph_stats_.get(); } + +void GraphOptimizer::LogOnDestruction(const bool log_on_destruction) { log_on_destruction_ = log_on_destruction; } + +bool GraphOptimizer::DoPostOptimizeActions() { return true; } + +bool GraphOptimizer::Update() { + LogDebug("Update: Updating."); + graph_stats_->update_timer_.Start(); + + graph_stats_->add_buffered_factors_timer_.Start(); + BufferCumulativeFactors(); + const int num_added_factors = AddBufferedFactors(); + graph_stats_->add_buffered_factors_timer_.Stop(); + if (num_added_factors <= 0) { + LogDebug("Update: No factors added."); + return false; + } + + // Only get marginals and slide window if optimization has already occured + // TODO(rsoussan): Make cleaner way to check for this + if (last_latest_time_) { + graph_stats_->marginals_timer_.Start(); + // Calculate marginals for covariances + try { + marginals_ = gtsam::Marginals(graph_, *values_, marginals_factorization_); + } catch (gtsam::IndeterminantLinearSystemException) { + log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); + marginals_ = boost::none; + } catch (const std::exception& exception) { + log(params_.fatal_failures, "Update: Computing marginals failed. " + std::string(exception.what())); + marginals_ = boost::none; + } catch (...) { + log(params_.fatal_failures, "Update: Computing marginals failed."); + marginals_ = boost::none; + } + graph_stats_->marginals_timer_.Stop(); + + graph_stats_->slide_window_timer_.Start(); + if (!SlideWindow(marginals_, *last_latest_time_)) { + LogError("Update: Failed to slide window."); + return false; + } + graph_stats_->slide_window_timer_.Stop(); + } + + // TODO(rsoussan): Is ordering required? if so clean these calls open and unify with marginalization + // TODO(rsoussan): Remove this now that marginalization occurs before optimization? + if (params_.add_marginal_factors) { + // Add graph ordering to place keys that will be marginalized in first group + const auto new_oldest_time = SlideWindowNewOldestTime(); + if (new_oldest_time) { + const auto old_keys = OldKeys(*new_oldest_time); + const auto ordering = gtsam::Ordering::ColamdConstrainedFirst(graph_, old_keys); + levenberg_marquardt_params_.setOrdering(ordering); + } else { + levenberg_marquardt_params_.orderingType = gtsam::Ordering::COLAMD; + } + } + + if (!ValidGraph()) { + LogError("Update: Invalid graph, not optimizing."); + return false; + } + + // Optimize + gtsam::LevenbergMarquardtOptimizer optimizer(graph_, *values_, levenberg_marquardt_params_); + + graph_stats_->optimization_timer_.Start(); + // TODO(rsoussan): Indicate if failure occurs in state msg, perhaps using confidence value in msg + try { + *values_ = optimizer.optimize(); + } catch (gtsam::IndeterminantLinearSystemException) { + log(params_.fatal_failures, "Update: Graph optimization failed, indeterminant linear system, keeping old values."); + } catch (gtsam::InvalidNoiseModel) { + log(params_.fatal_failures, "Update: Graph optimization failed, invalid noise model, keeping old values."); + } catch (gtsam::InvalidMatrixBlock) { + log(params_.fatal_failures, "Update: Graph optimization failed, invalid matrix block, keeping old values."); + } catch (gtsam::InvalidDenseElimination) { + log(params_.fatal_failures, "Update: Graph optimization failed, invalid dense elimination, keeping old values."); + } catch (...) { + log(params_.fatal_failures, "Update: Graph optimization failed, keeping old values."); + } + graph_stats_->optimization_timer_.Stop(); + + // Calculate marginals after the first optimization iteration so covariances + // can be used for first loc msg + // TODO(rsoussan): Clean this up + if (!last_latest_time_) { + graph_stats_->marginals_timer_.Start(); + // Calculate marginals for covariances + try { + marginals_ = gtsam::Marginals(graph_, *values_, marginals_factorization_); + } catch (gtsam::IndeterminantLinearSystemException) { + log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); + marginals_ = boost::none; + } catch (...) { + log(params_.fatal_failures, "Update: Computing marginals failed."); + marginals_ = boost::none; + } + graph_stats_->marginals_timer_.Stop(); + } + + last_latest_time_ = LatestTimestamp(); + + graph_stats_->log_stats_timer_.Start(); + graph_stats_->iterations_averager_.Update(optimizer.iterations()); + graph_stats_->UpdateStats(graph_); + graph_stats_->log_stats_timer_.Stop(); + graph_stats_->log_error_timer_.Start(); + graph_stats_->UpdateErrors(graph_); + graph_stats_->log_error_timer_.Stop(); + + if (params_.print_factor_info) PrintFactorDebugInfo(); + if (!DoPostOptimizeActions()) { + LogError("Update: Failed to complete post optimize actions."); + return false; + } + graph_stats_->update_timer_.Stop(); + return true; +} +} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/graph_stats.cc b/localization/graph_optimizer/src/graph_stats.cc new file mode 100644 index 0000000000..5f569c8a1e --- /dev/null +++ b/localization/graph_optimizer/src/graph_stats.cc @@ -0,0 +1,67 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include + +#include + +namespace graph_optimizer { +GraphStats::GraphStats() { + timers_.emplace_back(optimization_timer_); + timers_.emplace_back(update_timer_); + timers_.emplace_back(marginals_timer_); + timers_.emplace_back(slide_window_timer_); + timers_.emplace_back(add_buffered_factors_timer_); + timers_.emplace_back(log_error_timer_); + timers_.emplace_back(log_stats_timer_); + + AddStatsAverager(iterations_averager_); + AddErrorAverager(total_error_averager_); +} + +void GraphStats::AddStatsAverager(localization_common::Averager& stats_averager) { + stats_averagers_.emplace_back(stats_averager); +} + +void GraphStats::AddErrorAverager(localization_common::Averager& error_averager) { + error_averagers_.emplace_back(error_averager); +} + +void GraphStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) {} + +void GraphStats::UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) {} + +void GraphStats::Log() const { + Log(timers_); + Log(stats_averagers_); + Log(error_averagers_); +} + +void GraphStats::LogToFile(std::ofstream& ofstream) const { + LogToFile(timers_, ofstream); + LogToFile(stats_averagers_, ofstream); + LogToFile(error_averagers_, ofstream); +} + +void GraphStats::LogToCsv(std::ofstream& ofstream) const { + ofstream << "name,avg,min,max,stddev" << std::endl; + LogToCsv(timers_, ofstream); + LogToCsv(stats_averagers_, ofstream); + LogToCsv(error_averagers_, ofstream); +} +} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/graph_values.cc b/localization/graph_optimizer/src/graph_values.cc new file mode 100644 index 0000000000..a9483caec1 --- /dev/null +++ b/localization/graph_optimizer/src/graph_values.cc @@ -0,0 +1,29 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +namespace graph_optimizer { +namespace lc = localization_common; +namespace lm = localization_measurements; +GraphValues::GraphValues(std::shared_ptr values) : values_(std::move(values)) {} + +const gtsam::Values& GraphValues::values() const { return *values_; } + +gtsam::Values& GraphValues::values() { return *values_; } +} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/parameter_reader.cc b/localization/graph_optimizer/src/parameter_reader.cc new file mode 100644 index 0000000000..36fb3e4871 --- /dev/null +++ b/localization/graph_optimizer/src/parameter_reader.cc @@ -0,0 +1,36 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +namespace graph_optimizer { +namespace mc = msg_conversions; + +void LoadGraphOptimizerParams(config_reader::ConfigReader& config, GraphOptimizerParams& params) { + params.verbose = mc::LoadBool(config, "verbose"); + params.fatal_failures = mc::LoadBool(config, "fatal_failures"); + params.print_factor_info = mc::LoadBool(config, "print_factor_info"); + params.use_ceres_params = mc::LoadBool(config, "use_ceres_params"); + params.max_iterations = mc::LoadInt(config, "max_iterations"); + params.marginals_factorization = mc::LoadString(config, "marginals_factorization"); + params.add_marginal_factors = mc::LoadBool(config, "add_marginal_factors"); + params.huber_k = mc::LoadDouble(config, "huber_k"); + params.log_rate = mc::LoadInt(config, "log_rate"); +} +} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/utilities.cc b/localization/graph_optimizer/src/utilities.cc new file mode 100644 index 0000000000..db7eb6c301 --- /dev/null +++ b/localization/graph_optimizer/src/utilities.cc @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +namespace graph_optimizer { +gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& graph) { + gtsam::NonlinearFactorGraph removed_factors; + if (old_keys.empty()) return removed_factors; + + for (auto factor_it = graph.begin(); factor_it != graph.end();) { + bool found_key = false; + for (const auto& key : old_keys) { + if ((*factor_it)->find(key) != (*factor_it)->end()) { + found_key = true; + break; + } + } + if (found_key) { + removed_factors.push_back(*factor_it); + factor_it = graph.erase(factor_it); + } else { + ++factor_it; + } + } + + return removed_factors; +} +} // namespace graph_optimizer diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h index eec5741255..c82d78a521 100644 --- a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h @@ -51,6 +51,8 @@ class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerNodelet { bool SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); + bool DefaultServiceResponse(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + void PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose); void TwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist); @@ -62,10 +64,10 @@ class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerNodelet { boost::optional twist_; int input_mode_ = ff_msgs::SetEkfInputRequest::MODE_TRUTH; ros::Subscriber pose_sub_, twist_sub_; - ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_; + ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_, reset_pub_; ff_msgs::Heartbeat heartbeat_; tf2_ros::TransformBroadcaster transform_pub_; - ros::ServiceServer input_mode_srv_; + ros::ServiceServer input_mode_srv_, bias_srv_, bias_from_file_srv_, reset_srv_; }; } // namespace ground_truth_localizer diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc index befe94c8a0..bcd33eb3d8 100644 --- a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc @@ -24,12 +24,15 @@ #include +#include + namespace ground_truth_localizer { namespace lc = localization_common; -GroundTruthLocalizerNodelet::GroundTruthLocalizerNodelet() - : ff_util::FreeFlyerNodelet(NODE_SIM_LOC, true), platform_name_(GetPlatform()) {} +GroundTruthLocalizerNodelet::GroundTruthLocalizerNodelet() : ff_util::FreeFlyerNodelet(NODE_SIM_LOC, true) {} void GroundTruthLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); ff_common::InitFreeFlyerApplication(getMyArgv()); SubscribeAndAdvertise(nh); } @@ -39,6 +42,7 @@ void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { twist_pub_ = nh->advertise(TOPIC_LOCALIZATION_TWIST, 1); state_pub_ = nh->advertise(TOPIC_GNC_EKF, 1); heartbeat_pub_ = nh->advertise(TOPIC_HEARTBEAT, 5, true); + reset_pub_ = nh->advertise(TOPIC_GNC_EKF_RESET, 10); pose_sub_ = nh->subscribe(TOPIC_LOCALIZATION_TRUTH, 1, &GroundTruthLocalizerNodelet::PoseCallback, this, ros::TransportHints().tcpNoDelay()); @@ -46,6 +50,11 @@ void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { ros::TransportHints().tcpNoDelay()); input_mode_srv_ = nh->advertiseService(SERVICE_GNC_EKF_SET_INPUT, &GroundTruthLocalizerNodelet::SetMode, this); + bias_srv_ = + nh->advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); + bias_from_file_srv_ = nh->advertiseService(SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, + &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); + reset_srv_ = nh->advertiseService(SERVICE_GNC_EKF_RESET, &GroundTruthLocalizerNodelet::DefaultServiceResponse, this); } bool GroundTruthLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { @@ -53,6 +62,11 @@ bool GroundTruthLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff return true; } +bool GroundTruthLocalizerNodelet::DefaultServiceResponse(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) { + return true; +} + void GroundTruthLocalizerNodelet::PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose) { assert(pose->header.frame_id == "world"); pose_ = PoseFromMsg(*pose); diff --git a/localization/imu_augmentor/src/imu_augmentor_nodelet.cc b/localization/imu_augmentor/src/imu_augmentor_nodelet.cc index 21d370b9bd..942f7648cd 100644 --- a/localization/imu_augmentor/src/imu_augmentor_nodelet.cc +++ b/localization/imu_augmentor/src/imu_augmentor_nodelet.cc @@ -27,8 +27,7 @@ namespace imu_augmentor { namespace lc = localization_common; -ImuAugmentorNodelet::ImuAugmentorNodelet() - : ff_util::FreeFlyerNodelet(NODE_IMU_AUG, true), platform_name_(GetPlatform()) { +ImuAugmentorNodelet::ImuAugmentorNodelet() : ff_util::FreeFlyerNodelet(NODE_IMU_AUG, true) { imu_nh_.setCallbackQueue(&imu_queue_); loc_nh_.setCallbackQueue(&loc_queue_); heartbeat_.node = GetName(); @@ -36,6 +35,10 @@ ImuAugmentorNodelet::ImuAugmentorNodelet() } void ImuAugmentorNodelet::Initialize(ros::NodeHandle* nh) { + // Setup the platform name + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); + SubscribeAndAdvertise(nh); Run(); } diff --git a/localization/imu_integration/src/imu_integrator.cc b/localization/imu_integration/src/imu_integrator.cc index 5198d532aa..a1e7d8b44b 100644 --- a/localization/imu_integration/src/imu_integrator.cc +++ b/localization/imu_integration/src/imu_integrator.cc @@ -105,10 +105,18 @@ boost::optional ImuIntegrator::IntegrateImuMeasurements(const lc::Time } void ImuIntegrator::RemoveOldMeasurements(const lc::Time new_start_time) { - for (auto measurement_it = measurements_.cbegin(); - measurement_it != measurements_.cend() && measurement_it->first < new_start_time;) { - measurement_it = measurements_.erase(measurement_it); + const auto lower_bound_it = measurements_.lower_bound(new_start_time); + // Every element is too old + if (lower_bound_it == measurements_.cend()) { + measurements_.clear(); + return; } + // No elements are too old + if (lower_bound_it == measurements_.cbegin()) return; + + // Keep one before new_start_time so measurements before lower_bound_it can be interpolated if necessary + const auto new_oldest_measurement_it = std::prev(lower_bound_it); + measurements_.erase(measurements_.begin(), new_oldest_measurement_it); } boost::optional ImuIntegrator::IntegratedPim( diff --git a/localization/localization_common/include/localization_common/feature_point_3d.h b/localization/localization_common/include/localization_common/feature_point_3d.h new file mode 100644 index 0000000000..c440991630 --- /dev/null +++ b/localization/localization_common/include/localization_common/feature_point_3d.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ +#define LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ + +#include + +#include + +namespace localization_common { +using FeatureId = int; + +struct FeaturePoint3dNoise { + gtsam::SharedNoiseModel noise; +}; + +struct FeaturePoint3d { + FeaturePoint3d(const gtsam::Point3& global_t_point, const FeatureId feature_id) + : global_t_point(global_t_point), feature_id(feature_id) {} + FeaturePoint3d() {} + gtsam::Point3 global_t_point; + FeatureId feature_id; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(feature_id); + ar& BOOST_SERIALIZATION_NVP(global_t_point); + } +}; + +using FeaturePoint3ds = std::vector; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_FEATURE_POINT_3D_H_ diff --git a/localization/localization_node/tools/merge_bags.cc b/localization/localization_node/tools/merge_bags.cc new file mode 100644 index 0000000000..acd00d8456 --- /dev/null +++ b/localization/localization_node/tools/merge_bags.cc @@ -0,0 +1,114 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * https://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include +#include + +#include + +#include +#include + +// ROS +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +// Merge bags +// Usage: merge_bags -output_bag output.bag input1.bag input2.bag ... + +DEFINE_string(output_bag, "", + "The output bag."); + +// Read the list of topics in a bag while avoiding repetitions +void readTopicsInBag(std::string const& bag_file, std::vector & topics) { + topics.clear(); + + rosbag::Bag bag(bag_file.c_str()); + rosbag::View view(bag); + std::vector connection_infos = view.getConnections(); + + // Read first in a set to deal with any repetitions + std::set topics_set; + BOOST_FOREACH(const rosbag::ConnectionInfo *info, connection_infos) { + topics_set.insert(info->topic); + } + + // Copy the unique names to a vector + for (auto it = topics_set.begin(); it != topics_set.end() ; it++) { + topics.push_back(*it); + } +} + +int main(int argc, char ** argv) { + ff_common::InitFreeFlyerApplication(&argc, &argv); + + if (FLAGS_output_bag == "") { + std::cout << "The output bag was not specified.\n"; + return 1; + } + + // Make the directory where the output will go + std::string out_dir = boost::filesystem::path(FLAGS_output_bag).parent_path().string(); + if (out_dir == "") + out_dir = "."; + if (!boost::filesystem::exists(out_dir)) { + int status = mkdir(out_dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); + if (status && errno != EEXIST) { + std::cout << "Failed to create directory: " << out_dir << "."; + return 1; + } + } + + // Open the output bag + std::cout << "Opening the output bag file: " << FLAGS_output_bag << "\n"; + rosbag::Bag output_bag; + output_bag.open(FLAGS_output_bag, rosbag::bagmode::Write); + + // Append the input bags + for (int it = 1; it < argc; it++) { + std::string input_bag_file = argv[it]; + + std::cout << "Opening the input bag file: " << input_bag_file << "\n"; + + // Get the topics from the input bag + std::vector topics; + readTopicsInBag(input_bag_file, topics); + + rosbag::Bag input_bag; + input_bag.open(input_bag_file, rosbag::bagmode::Read); + + rosbag::View view(input_bag, rosbag::TopicQuery(topics)); + for (rosbag::MessageInstance const m : view) { + output_bag.write(m.getTopic(), m.getTime(), m); + } + + input_bag.close(); + } + + output_bag.close(); + + return 0; +} diff --git a/localization/sparse_mapping/build_map.md b/localization/sparse_mapping/build_map.md index 316c4efe1a..216c821687 100644 --- a/localization/sparse_mapping/build_map.md +++ b/localization/sparse_mapping/build_map.md @@ -1,4 +1,4 @@ -# Map building +\page map_building Map building Here we describe how to build a map. diff --git a/localization/sparse_mapping/granite_lab_registration.md b/localization/sparse_mapping/granite_lab_registration.md index e4e956ddb1..46773ce0f8 100644 --- a/localization/sparse_mapping/granite_lab_registration.md +++ b/localization/sparse_mapping/granite_lab_registration.md @@ -1,3 +1,5 @@ +\page granite_lab_registration Granite Lab Registration + # Locations of the control points in the granite lab used for registration The coordinates of four points on the granite table were measured. They are specified here: diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index 551921ff5c..9e9aa4e4cc 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -1,3 +1,5 @@ +\page sparsemapping Sparse Mapping + # Creation of sparse maps for robot localization ## What is a map @@ -68,9 +70,8 @@ The bags created on the ISS are likely split into many smaller bags, for easy and reliability of transfer. Those can be merged into one bag as follows: - source $BUILD_PATH/devel/setup.bash - python $SOURCE_PATH/localization/sparse_mapping/tools/merge_bags.py \ - --verbose + astrobee_build/devel/lib/localization_node/merge_bags \ + -output_bag ### Extracting images @@ -479,3 +480,9 @@ Instead of taking images out of the map randomly, one can start with a reduced map with a small list of desired images which can be set with -image_list, and then all images for which localization fails will be added back to it. + + +\subpage map_building +\subpage total_station +\subpage granite_lab_registration +\subpage using_faro \ No newline at end of file diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 5766d82fd1..19bca44684 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -391,6 +391,14 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { histogram_equalization_ == 1 || histogram_equalization_ == 2); + // For backward compatibility with old maps, allow a map to have its + // histogram_equalization flag unspecified, but it is best to avoid + // that situation, and rebuild the map if necessary. + if (histogram_equalization_ == 2) + std::cout << "Warning: Unknown value of histogram_equalization! " + << "It is strongly suggested to rebuild this map to avoid " + << "poor quality results." << std::endl; + delete input; close(input_fd); } @@ -402,6 +410,14 @@ void SparseMap::SetDetectorParams(int min_features, int max_features, int retrie } void SparseMap::Save(const std::string & protobuf_file) const { + // For backward compatibility with old maps, allow a map to have its + // histogram_equalization flag unspecified, but it is best to avoid + // that situation, and rebuild the map if necessary. + if (histogram_equalization_ == 2) + std::cout << "Warning: Unknown value of histogram_equalization! " + << "It is strongly suggested to rebuild this map to avoid " + << "poor quality results." << std::endl; + sparse_mapping_protobuf::Map map; map.set_detector_name(detector_.GetDetectorName()); if (!cid_to_descriptor_map_.empty()) diff --git a/localization/sparse_mapping/tools/merge_bags.py b/localization/sparse_mapping/tools/merge_bags.py deleted file mode 100644 index 2ae3eea1d7..0000000000 --- a/localization/sparse_mapping/tools/merge_bags.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Merge bags. -# Usage: -# python merge_bags.py output.bag input*bag - -import sys -import argparse -from fnmatch import fnmatchcase - -from rosbag import Bag - -def main(): - - parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.') - parser.add_argument('outputbag', help='Output bag file with topics merged.') - parser.add_argument('inputbag', nargs='+', help='Input bag files.') - parser.add_argument('-v', '--verbose', action="store_true", default=False, - help='Verbose output.') - parser.add_argument('-t', '--topics', default="*", - help='String interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file.') - - args = parser.parse_args() - - topics = args.topics.split(' ') - - total_included_count = 0 - total_skipped_count = 0 - - if (args.verbose): - print("Writing bag file: " + args.outputbag) - print("Matching topics against patters: '%s'" % ' '.join(topics)) - - with Bag(args.outputbag, 'w') as o: - for ifile in args.inputbag: - matchedtopics = [] - included_count = 0 - skipped_count = 0 - if (args.verbose): - print("> Reading bag file: " + ifile) - with Bag(ifile, 'r') as ib: - for topic, msg, t in ib: - if any(fnmatchcase(topic, pattern) for pattern in topics): - if not topic in matchedtopics: - matchedtopics.append(topic) - if (args.verbose): - print("Including matched topic '%s'" % topic) - o.write(topic, msg, t) - included_count += 1 - else: - skipped_count += 1 - total_included_count += included_count - total_skipped_count += skipped_count - if (args.verbose): - print("< Included %d messages and skipped %d" % (included_count, skipped_count)) - - if (args.verbose): - print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count)) - -if __name__ == "__main__": - main() diff --git a/localization/sparse_mapping/tools/merge_maps.cc b/localization/sparse_mapping/tools/merge_maps.cc index cc630831d2..ac8768bc4a 100644 --- a/localization/sparse_mapping/tools/merge_maps.cc +++ b/localization/sparse_mapping/tools/merge_maps.cc @@ -43,7 +43,7 @@ // merging may move things around a bit. // outputs -DEFINE_string(output_map, "merged.map", +DEFINE_string(output_map, "", "Output file containing the merged map."); DEFINE_int32(num_image_overlaps_at_endpoints, 10, @@ -74,6 +74,9 @@ int main(int argc, char** argv) { LOG(FATAL) << "The input and output maps must have different names."; } + if (FLAGS_output_map == "") + LOG(FATAL) << "No output map was specified."; + if (FLAGS_num_image_overlaps_at_endpoints <= 0) LOG(FATAL) << "Must have num_image_overlaps_at_endpoints > 0."; diff --git a/localization/sparse_mapping/total_station.md b/localization/sparse_mapping/total_station.md index f656404317..e58f737998 100644 --- a/localization/sparse_mapping/total_station.md +++ b/localization/sparse_mapping/total_station.md @@ -1,3 +1,5 @@ +\page total_station Total Station + # Doing a Survey with the Total Station This will tell you how to just operate the Total Station device. When diff --git a/management/executive/include/executive/executive.h b/management/executive/include/executive/executive.h index 094b709548..63bac47bb9 100644 --- a/management/executive/include/executive/executive.h +++ b/management/executive/include/executive/executive.h @@ -200,10 +200,8 @@ class Executive : public ff_util::FreeFlyerNodelet { // Commands bool ArmPanAndTilt(ff_msgs::CommandStampedPtr const& cmd); bool AutoReturn(ff_msgs::CommandStampedPtr const& cmd); - bool ClearData(ff_msgs::CommandStampedPtr const& cmd); bool CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd); bool Dock(ff_msgs::CommandStampedPtr const& cmd); - bool DownloadData(ff_msgs::CommandStampedPtr const& cmd); bool Fault(ff_msgs::CommandStampedPtr const& cmd); bool GripperControl(ff_msgs::CommandStampedPtr const& cmd); bool IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd); @@ -225,6 +223,7 @@ class Executive : public ff_util::FreeFlyerNodelet { bool SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd); bool SetEnableAutoReturn(ff_msgs::CommandStampedPtr const& cmd); bool SetEnableImmediate(ff_msgs::CommandStampedPtr const& cmd); + bool SetEnableReplan(ff_msgs::CommandStampedPtr const& cmd); bool SetFlashlightBrightness(ff_msgs::CommandStampedPtr const& cmd); bool SetHolonomicMode(ff_msgs::CommandStampedPtr const& cmd); bool SetInertia(ff_msgs::CommandStampedPtr const& cmd); @@ -232,15 +231,12 @@ class Executive : public ff_util::FreeFlyerNodelet { bool SetPlan(ff_msgs::CommandStampedPtr const& cmd); bool SetPlanner(ff_msgs::CommandStampedPtr const& cmd); bool SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd); - bool SetTimeSync(ff_msgs::CommandStampedPtr const& cmd); bool SetZones(ff_msgs::CommandStampedPtr const& cmd); - bool Shutdown(ff_msgs::CommandStampedPtr const& cmd); bool SkipPlanStep(ff_msgs::CommandStampedPtr const& cmd); bool StartGuestScience(ff_msgs::CommandStampedPtr const& cmd); bool StartRecording(ff_msgs::CommandStampedPtr const& cmd); bool StopAllMotion(ff_msgs::CommandStampedPtr const& cmd); bool StopArm(ff_msgs::CommandStampedPtr const& cmd); - bool StopDownload(ff_msgs::CommandStampedPtr const& cmd); bool StopRecording(ff_msgs::CommandStampedPtr const& cmd); bool StopGuestScience(ff_msgs::CommandStampedPtr const& cmd); bool StowArm(ff_msgs::CommandStampedPtr const& cmd); @@ -249,7 +245,6 @@ class Executive : public ff_util::FreeFlyerNodelet { bool Unperch(ff_msgs::CommandStampedPtr const& cmd); bool Unterminate(ff_msgs::CommandStampedPtr const& cmd); bool Wait(ff_msgs::CommandStampedPtr const& cmd); - bool WipeHlp(ff_msgs::CommandStampedPtr const& cmd); protected: virtual void Initialize(ros::NodeHandle *nh); diff --git a/management/executive/src/executive.cc b/management/executive/src/executive.cc index 2b2c69e3f5..20cc6961f8 100644 --- a/management/executive/src/executive.cc +++ b/management/executive/src/executive.cc @@ -1235,15 +1235,15 @@ bool Executive::ConfigureMobility(std::string const& cmd_id) { choreographer_cfg_->Set("enable_collision_checking", agent_state_.check_obstacles); choreographer_cfg_->Set("enable_validation", agent_state_.check_zones); - choreographer_cfg_->Set("enable_timesync", - agent_state_.time_sync_enabled); + choreographer_cfg_->Set("enable_timesync", false); choreographer_cfg_->Set("enable_immediate", agent_state_.immediate_enabled); choreographer_cfg_->Set("planner", agent_state_.planner); // This function is not used for the first segment of a plan so always disable // move to start choreographer_cfg_->Set("enable_bootstrapping", false); - choreographer_cfg_->Set("enable_replanning", false); + choreographer_cfg_->Set("enable_replanning", + agent_state_.replanning_enabled); // Mapper mapper_cfg_->Set("inflate_radius", agent_state_.collision_distance); @@ -1302,13 +1302,13 @@ bool Executive::ConfigureMobility(bool move_to_start, choreographer_cfg_->Set("enable_collision_checking", agent_state_.check_obstacles); choreographer_cfg_->Set("enable_validation", agent_state_.check_zones); - choreographer_cfg_->Set("enable_timesync", - agent_state_.time_sync_enabled); + choreographer_cfg_->Set("enable_timesync", false); choreographer_cfg_->Set("enable_immediate", agent_state_.immediate_enabled); choreographer_cfg_->Set("planner", agent_state_.planner); choreographer_cfg_->Set("enable_bootstrapping", move_to_start); - choreographer_cfg_->Set("enable_replanning", false); + choreographer_cfg_->Set("enable_replanning", + agent_state_.replanning_enabled); // Mapper mapper_cfg_->Set("inflate_radius", agent_state_.collision_distance); @@ -1557,42 +1557,6 @@ bool Executive::AutoReturn(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::ClearData(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing clear data command!"); - bool successful = true; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; - std::string err_msg = ""; - - // Don't clear data when flying, docking, perching, or trying to stop - if (!CheckNotMoving(cmd)) { - return false; - } - - // Check to make sure command is formatted as expected - if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { - successful = false; - err_msg = "Malformed arguments for clear data command!"; - } else if (cmd->args[0].s != - CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_IMMEDIATE - && cmd->args[0].s != - CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_DELAYED) { - successful = false; - err_msg = "Data method not recognized. Needs to be immediate or delay."; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; - } else { - // TODO(Katie) Stub, change to be actual code, including setting a class - // variable to tell if we are downloading data, cannot clear data if - // downloading data - successful = true; - NODELET_ERROR("Clear data not implemented yet!"); - } - - state_->AckCmd(cmd->cmd_id, completed_status, err_msg); - - return successful; -} - bool Executive::CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing custom guest science command!"); // Check command arguments are correcy before sending to the guest science @@ -1654,46 +1618,6 @@ bool Executive::Dock(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::DownloadData(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing download data command!"); - bool successful = true; - std::string err_msg = ""; - uint8_t completed_status; - - // Check to make sure command is formatted as expected - if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { - successful = false; - err_msg = "Malformed arguments for download data command!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; - } else if (cmd->args[0].s != - CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_IMMEDIATE - && cmd->args[0].s != - CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_DELAYED) { - successful = false; - err_msg = "Download method not recognized. Needs to be immediate or delay."; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; - } else if (agent_state_.mobility_state.state != - ff_msgs::MobilityState::DOCKING || - (agent_state_.mobility_state.state == - ff_msgs::MobilityState::DOCKING && - agent_state_.mobility_state.sub_state != 0)) { - // Can only download data when docked - successful = false; - err_msg = "Not docked! Need to be docked in order to download data."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; - } else { - // TODO(Katie) Stub, change to be actual code, including setting a class - // variable to tell if we are downloading data and what kind of data - successful = true; - NODELET_ERROR("Download data not implemented yet!"); - completed_status = ff_msgs::AckCompletedStatus::OK; - } - - state_->AckCmd(cmd->cmd_id, completed_status, err_msg); - return successful; -} - bool Executive::Fault(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing fault command!"); // Only transition to the fault state if the fault command came from the @@ -2462,6 +2386,26 @@ bool Executive::SetEnableImmediate(ff_msgs::CommandStampedPtr const& cmd) { return false; } +bool Executive::SetEnableReplan(ff_msgs::CommandStampedPtr const& cmd) { + NODELET_INFO("Executive executing set enable replan command!"); + if (CheckNotMoving(cmd)) { + if (cmd->args.size() != 1 || + cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { + NODELET_ERROR("Malformed arguments for enable replan command!"); + state_->AckCmd(cmd->cmd_id, + ff_msgs::AckCompletedStatus::BAD_SYNTAX, + "Malformed arguments for enable replan command!"); + return false; + } + + agent_state_.replanning_enabled = cmd->args[0].b; + PublishAgentState(); + state_->AckCmd(cmd->cmd_id); + return true; + } + return false; +} + bool Executive::SetFlashlightBrightness(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing set flashlight brightness command!"); bool successful = true; @@ -2794,26 +2738,6 @@ bool Executive::SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::SetTimeSync(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set time sync command!"); - // Don't set time sync when moving - if (CheckNotMoving(cmd)) { - if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, - "Malformed arguments for enable time sync command!"); - return false; - } - - agent_state_.time_sync_enabled = cmd->args[0].b; - PublishAgentState(); - state_->AckCmd(cmd->cmd_id); - return true; - } - return false; -} - bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing set zones command!"); if (CheckNotMoving(cmd)) { @@ -2967,20 +2891,6 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::Shutdown(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing shutdown command!"); - // Don't want to shutdown when flying, docking, perching, or trying to stop - if (CheckNotMoving(cmd)) { - // TODO(Katie) Stub, change to be actual code, ack complete immediately - // TODO(Katie) Add code to shutdown the robot - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - "Shutdown not implemented yet! Stay tune!"); - return false; - } - return false; -} - bool Executive::SkipPlanStep(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing skip plan step command!"); // Make sure plan execution state is paused @@ -3293,43 +3203,6 @@ bool Executive::StopArm(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::StopDownload(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stop download command!"); - std::string err_msg; - // Check to make sure command is formatted as expected - if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { - err_msg = "Malformed arguments for stop download command!"; - NODELET_ERROR("%s", err_msg.c_str()); - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, - err_msg); - return false; - } - - if (cmd->args[0].s != CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_IMMEDIATE - && cmd->args[0].s != - CommandConstants::PARAM_NAME_DOWNLOAD_METHOD_DELAYED) { - err_msg = "Download method not recognized. Needs to be immediate or delay."; - NODELET_ERROR("%s", err_msg.c_str()); - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, - err_msg); - return false; - } - - // TODO(Katie) Can only stop download if download occurring, check class - // variables to see if a download is in progress and what kind of data - // TODO(Katie) Stub, change to be actual code - err_msg = "Stop download not implemented yet!"; - NODELET_ERROR("%s", err_msg.c_str()); - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - err_msg); - // err_msg = "Not downloading data! No download to stop."; - return false; -} - bool Executive::StopGuestScience(ff_msgs::CommandStampedPtr const& cmd) { NODELET_INFO("Executive executing stop guest science command!"); // Check command arguments are correct before sending to the guest science @@ -3552,15 +3425,6 @@ bool Executive::Wait(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::WipeHlp(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing wipe hlp command!"); - // TODO(Katie) Check if guest science apk is running. If so, don't wipe hlp. - state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - "Wipe hlp not implemented yet! Stay tune!"); - return false; -} - /************************ Protected *******************************************/ void Executive::Initialize(ros::NodeHandle *nh) { std::string err_msg; @@ -3828,7 +3692,7 @@ void Executive::Initialize(ros::NodeHandle *nh) { agent_state_.check_zones = true; agent_state_.auto_return_enabled = true; agent_state_.immediate_enabled = true; - agent_state_.time_sync_enabled = false; + agent_state_.replanning_enabled = false; agent_state_.boot_time = ros::Time::now().sec; PublishAgentState(); diff --git a/management/executive/src/op_state_auto_return.cc b/management/executive/src/op_state_auto_return.cc index c4176e422a..06dfae5d96 100644 --- a/management/executive/src/op_state_auto_return.cc +++ b/management/executive/src/op_state_auto_return.cc @@ -61,8 +61,6 @@ OpState* OpStateAutoReturn::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->StopArm(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_GUEST_SCIENCE) { exec_->StopGuestScience(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_WIPE_HLP) { - exec_->WipeHlp(cmd); } else { std::string err_msg = "Command " + cmd->cmd_name + " not accepted in op state auto return."; diff --git a/management/executive/src/op_state_fault.cc b/management/executive/src/op_state_fault.cc index 7015f51799..c79637b0fd 100644 --- a/management/executive/src/op_state_fault.cc +++ b/management/executive/src/op_state_fault.cc @@ -31,12 +31,8 @@ OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (cmd->cmd_name == CommandConstants::CMD_NAME_ARM_PAN_AND_TILT) { exec_->ArmPanAndTilt(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CLEAR_DATA) { - exec_->ClearData(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CUSTOM_GUEST_SCIENCE) { exec_->CustomGuestScience(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_DOWNLOAD_DATA) { - exec_->DownloadData(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_FAULT) { exec_->Fault(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_GRIPPER_CONTROL) { @@ -59,6 +55,8 @@ OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetCheckZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_IMMEDIATE) { exec_->SetEnableImmediate(cmd); + } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_REPLAN) { + exec_->SetEnableReplan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS) { exec_->SetFlashlightBrightness(cmd); @@ -72,18 +70,12 @@ OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetPlan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_PLANNER) { exec_->SetPlanner(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_TIME_SYNC) { - exec_->SetTimeSync(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ZONES) { exec_->SetZones(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SHUTDOWN) { - exec_->Shutdown(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_ALL_MOTION) { exec_->StopAllMotion(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_ARM) { exec_->StopArm(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_DOWNLOAD) { - exec_->StopDownload(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_GUEST_SCIENCE) { exec_->StopGuestScience(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOW_ARM) { @@ -92,8 +84,6 @@ OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SwitchLocalization(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_UNTERMINATE) { exec_->Unterminate(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_WIPE_HLP) { - exec_->WipeHlp(cmd); } else { err_msg = "Command " + cmd->cmd_name + " not accepted in op state" + " fault."; diff --git a/management/executive/src/op_state_plan_exec.cc b/management/executive/src/op_state_plan_exec.cc index 3f764cbba9..597d2c2fe7 100644 --- a/management/executive/src/op_state_plan_exec.cc +++ b/management/executive/src/op_state_plan_exec.cc @@ -79,12 +79,6 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (!exec_->ArmPanAndTilt(cmd)) { return OpStateRepo::Instance()->ready()->StartupState(); } - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CLEAR_DATA) { - if (exec_->ClearData(cmd)) { - return AckStartPlanItem(); - } else { - return OpStateRepo::Instance()->ready()->StartupState(); - } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CUSTOM_GUEST_SCIENCE) { if (!exec_->CustomGuestScience(cmd)) { @@ -94,12 +88,6 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (!exec_->Dock(cmd)) { return OpStateRepo::Instance()->ready()->StartupState(); } - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_DOWNLOAD_DATA) { - if (exec_->DownloadData(cmd)) { - return AckStartPlanItem(); - } else { - return OpStateRepo::Instance()->ready()->StartupState(); - } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_GRIPPER_CONTROL) { if (!exec_->GripperControl(cmd)) { return OpStateRepo::Instance()->ready()->StartupState(); @@ -277,8 +265,6 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_GUEST_SCIENCE) { exec_->StopGuestScience(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_WIPE_HLP) { - exec_->WipeHlp(cmd); } else { err_msg = "Command " + cmd->cmd_name + "not accepted in op state" + " plan execution."; diff --git a/management/executive/src/op_state_ready.cc b/management/executive/src/op_state_ready.cc index b24640768b..ecf63e6404 100644 --- a/management/executive/src/op_state_ready.cc +++ b/management/executive/src/op_state_ready.cc @@ -41,16 +41,12 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (exec_->AutoReturn(cmd)) { return OpStateRepo::Instance()->teleop()->StartupState(); } - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CLEAR_DATA) { - exec_->ClearData(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_CUSTOM_GUEST_SCIENCE) { exec_->CustomGuestScience(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_DOCK) { if (exec_->Dock(cmd)) { return OpStateRepo::Instance()->teleop()->StartupState(); } - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_DOWNLOAD_DATA) { - exec_->DownloadData(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_FAULT) { if (exec_->Fault(cmd)) { return OpStateRepo::Instance()->fault()->StartupState(); @@ -98,6 +94,8 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetCheckZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_IMMEDIATE) { exec_->SetEnableImmediate(cmd); + } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_REPLAN) { + exec_->SetEnableReplan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS) { exec_->SetFlashlightBrightness(cmd); @@ -111,12 +109,8 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetPlan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_PLANNER) { exec_->SetPlanner(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_TIME_SYNC) { - exec_->SetTimeSync(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ZONES) { exec_->SetZones(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SHUTDOWN) { - exec_->Shutdown(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF) { // Make sure we are stopped, not docked or perched, before moving if ((exec_->GetMobilityState().state == ff_msgs::MobilityState::STOPPING && @@ -155,8 +149,6 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (exec_->StopArm(cmd)) { return OpStateRepo::Instance()->teleop()->StartupState(); } - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_DOWNLOAD) { - exec_->StopDownload(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_GUEST_SCIENCE) { exec_->StopGuestScience(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOW_ARM) { @@ -177,8 +169,6 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_UNTERMINATE) { exec_->Unterminate(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_WIPE_HLP) { - exec_->WipeHlp(cmd); } else { err_msg = "Command " + cmd->cmd_name + " not accepted in operating state " + "ready."; diff --git a/management/executive/src/op_state_teleop.cc b/management/executive/src/op_state_teleop.cc index f39a387cff..5e5a066b4b 100644 --- a/management/executive/src/op_state_teleop.cc +++ b/management/executive/src/op_state_teleop.cc @@ -57,6 +57,8 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetCheckZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_IMMEDIATE) { exec_->SetEnableImmediate(cmd); + } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ENABLE_REPLAN) { + exec_->SetEnableReplan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS) { exec_->SetFlashlightBrightness(cmd); @@ -70,8 +72,6 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetPlan(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_PLANNER) { exec_->SetPlanner(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_TIME_SYNC) { - exec_->SetTimeSync(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SET_ZONES) { exec_->SetZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF) { @@ -112,16 +112,12 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->StopAllMotion(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_ARM) { exec_->StopArm(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_DOWNLOAD) { - exec_->StopDownload(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_GUEST_SCIENCE) { exec_->StopGuestScience(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOW_ARM) { exec_->StowArm(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SWITCH_LOCALIZATION) { exec_->SwitchLocalization(cmd); - } else if (cmd->cmd_name == CommandConstants::CMD_NAME_WIPE_HLP) { - exec_->WipeHlp(cmd); } else { err_msg = "Command " + cmd->cmd_name + " not accepted in op state" + " teleop."; diff --git a/management/executive/src/utils/sequencer/command_conversion.cc b/management/executive/src/utils/sequencer/command_conversion.cc index b5a493354c..9b68daa279 100644 --- a/management/executive/src/utils/sequencer/command_conversion.cc +++ b/management/executive/src/utils/sequencer/command_conversion.cc @@ -129,17 +129,6 @@ bool GenGuestScience(const jsonloader::Command *plan_cmd, return true; } -bool GenData(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { - const jsonloader::DataCommand *d_cmd = - dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; - arg.s = d_cmd->data_method(); - dds_cmd->args.push_back(arg); - return true; -} - bool GenCustomScience(const jsonloader::Command *plan_cmd, ff_msgs::CommandStamped *dds_cmd) { const jsonloader::CustomGuestScienceCommand *g_cmd = @@ -156,22 +145,6 @@ bool GenCustomScience(const jsonloader::Command *plan_cmd, return true; } -bool GenGeneric(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { - const jsonloader::GenericCommand *g_cmd = - dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; - arg.s = g_cmd->name(); - dds_cmd->args.push_back(arg); - - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; - arg2.s = g_cmd->param(); - dds_cmd->args.push_back(arg2); - return true; -} - bool GenSetCamera(const jsonloader::Command *plan_cmd, ff_msgs::CommandStamped *dds_cmd) { const jsonloader::SetCameraCommand *s_cmd = @@ -338,8 +311,6 @@ extern const std::unordered_map kCmdGenMap = { { jl::kCmdPowerOff, { cc::CMD_NAME_POWER_OFF_ITEM, cc::CMD_SUBSYS_POWER, GenPower } }, { jl::kCmdArmPanTilt, { cc::CMD_NAME_ARM_PAN_AND_TILT, cc::CMD_SUBSYS_ARM, GenArm } }, { jl::kCmdGripper, { cc::CMD_NAME_GRIPPER_CONTROL, cc::CMD_SUBSYS_ARM, GenGripper } }, - { jl::kCmdClearData, { cc::CMD_NAME_CLEAR_DATA, cc::CMD_SUBSYS_DATA, GenData } }, - { jl::kCmdDownload, { cc::CMD_NAME_DOWNLOAD_DATA, cc::CMD_SUBSYS_DATA, GenData } }, { jl::kCmdStartGuest, { cc::CMD_NAME_START_GUEST_SCIENCE, cc::CMD_SUBSYS_GUEST_SCIENCE, GenGuestScience } }, { jl::kCmdStopGuest, @@ -348,8 +319,6 @@ extern const std::unordered_map kCmdGenMap = { { cc::CMD_NAME_CUSTOM_GUEST_SCIENCE, cc::CMD_SUBSYS_GUEST_SCIENCE, GenCustomScience } }, { jl::kCmdFlashlight, { cc::CMD_NAME_SET_FLASHLIGHT_BRIGHTNESS, cc::CMD_SUBSYS_POWER, GenFlashlight } }, - { jl::kCmdGenericCmd, - { cc::CMD_NAME_GENERIC_COMMAND, cc::CMD_SUBSYS_SETTINGS, GenGeneric } }, { jl::kCmdIdleProp, { cc::CMD_NAME_IDLE_PROPULSION, cc::CMD_SUBSYS_MOBILITY, GenNoop } }, { jl::kCmdSetCamera, diff --git a/management/executive/tools/data_to_disk_pub.cc b/management/executive/tools/data_to_disk_pub.cc new file mode 100644 index 0000000000..74dbfaecde --- /dev/null +++ b/management/executive/tools/data_to_disk_pub.cc @@ -0,0 +1,160 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace fs = boost::filesystem; +namespace io = boost::iostreams; + +namespace flags = FREEFLYER_GFLAGS_NAMESPACE; + +DEFINE_string(compression, "none", + "Type of compression [none, deflate, gzip]"); + +constexpr uintmax_t kMaxSize = 128 * 1024; +ros::Time data_pub_time; +ros::Publisher command_pub; + +bool ValidateCompression(const char* name, std::string const &value) { + if (value == "none" || value == "gzip" || value == "deflate") + return true; + + std::cerr << "Compression must be one of: nono, deflate, or gzip" + << std::endl; + return false; +} + +void on_connect(ros::SingleSubscriberPublisher const& sub, + ff_msgs::CompressedFile &cf) { // NOLINT + ROS_INFO("subscriber present: sending data to disk"); + cf.header.stamp = ros::Time::now(); + sub.publish(cf); +} + +void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) { + ROS_INFO("ack received: sending set data to disk command"); + // compressed file ack is latched so we need to check the timestamp to make + // sure this plan is being acked + if (data_pub_time < cf_ack->header.stamp) { + ff_msgs::CommandStamped cmd; + cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_DATA_TO_DISK; + cmd.subsys_name = "Astrobee"; + command_pub.publish(cmd); + ros::shutdown(); + } +} + +int main(int argc, char** argv) { + ff_common::InitFreeFlyerApplication(&argc, &argv); + ros::init(argc, argv, "data_to_disk_publisher"); + + if (!flags::RegisterFlagValidator(&FLAGS_compression, &ValidateCompression)) { + std::cerr << "Failed to register compression flag validator." << std::endl; + return -1; + } + flags::ParseCommandLineFlags(&argc, &argv, true); + + if (argc <= 1) { + std::cerr << "error: must provide at least one file to send as zones" + << std::endl; + return -1; + } + + ff_msgs::CompressedFile cf; + cf.header.seq = 1; + cf.header.frame_id = "world"; + + // Load the plan + io::file_descriptor_source source(argv[1], std::ios::in | std::ios::binary); + std::vector data; + data.reserve(kMaxSize); + + io::filtering_ostream out; + if (FLAGS_compression == "deflate") { + cf.type = ff_msgs::CompressedFile::TYPE_DEFLATE; + out.push(io::zlib_compressor()); + } else if (FLAGS_compression == "gzip") { + cf.type = ff_msgs::CompressedFile::TYPE_GZ; + out.push(io::gzip_compressor()); + } else { + cf.type = ff_msgs::CompressedFile::TYPE_NONE; + } + out.push(io::back_inserter(data)); + + io::copy(source, out); + if (data.size() >= kMaxSize) { + std::cerr << "error: even after using a '" << FLAGS_compression + << "' compression, the file is too big." << std::endl + << "[file must be less that 128k in size, after compression]" + << std::endl; + return -1; + } + + // Copy the data into a CompressedFile (because char and 'unsigned' char) + // are totally different. + // NOTE: I also tried setting the msg to 'int8', but ROS made it 'signed', + // which was just as unhelpful. Thus, copying. + cf.file.reserve(data.size()); + for (char c : data) { + cf.file.push_back(static_cast(c)); + } + + ros::NodeHandle n; + data_pub_time = ros::Time::now(); + + // Publishes the data to disk to the executive using subscriber status + // callbacks + ros::Publisher data_to_disk_pub = n.advertise( + TOPIC_COMMUNICATIONS_DDS_DATA, 10, + std::bind(&on_connect, std::placeholders::_1, cf)); + + // After the zones are received, commands a set zones to the executive + command_pub = n.advertise( + TOPIC_COMMAND, 5, true); + + // Subscriber that receives confirmation that the zones were received + ros::Subscriber cf_acK_pub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); + + ROS_INFO("waiting for a subscriber..."); + ros::spin(); + + return 0; +} diff --git a/mobility/choreographer/src/choreographer_nodelet.cc b/mobility/choreographer/src/choreographer_nodelet.cc index a97ccbf814..c3db2188b2 100644 --- a/mobility/choreographer/src/choreographer_nodelet.cc +++ b/mobility/choreographer/src/choreographer_nodelet.cc @@ -1118,6 +1118,11 @@ class ChoreographerNodelet : public ff_util::FreeFlyerNodelet { ros::Time reftime = goal.segment.front().when; if (cfg_.Get("enable_immediate")) reftime = ros::Time::now(); + // Timesync was an attempt to synchronize Astrobees without Astrobee to + // Astrobee communication. It is not recommended to enable it since it + // hasn't been tested and the synchronization protocol is not robust; one + // can not guarentee that the different Astrobees will receive the + // command within the same discrete time unit window. if (cfg_.Get("enable_timesync")) { reftime.sec += cfg_.Get("discrete_time_unit"); reftime.sec += reftime.sec % cfg_.Get("discrete_time_unit"); diff --git a/scripts/docker/astrobee_base_kinetic.Dockerfile b/scripts/docker/astrobee_base_kinetic.Dockerfile index db45423629..08bc1486e3 100644 --- a/scripts/docker/astrobee_base_kinetic.Dockerfile +++ b/scripts/docker/astrobee_base_kinetic.Dockerfile @@ -42,7 +42,7 @@ RUN apt-get update \ COPY ./scripts/setup/packages_*.lst /setup/astrobee/ # note apt-get update is run within the following shell script -RUN /setup/astrobee/install_desktop_16_04_packages.sh \ +RUN /setup/astrobee/install_desktop_packages.sh \ && rm -rf /var/lib/apt/lists/* #Add new sudo user diff --git a/scripts/docker/astrobee_base_melodic.Dockerfile b/scripts/docker/astrobee_base_melodic.Dockerfile index ad43aaec92..f0a0bf2d7a 100755 --- a/scripts/docker/astrobee_base_melodic.Dockerfile +++ b/scripts/docker/astrobee_base_melodic.Dockerfile @@ -38,67 +38,16 @@ RUN apt-get update \ python-rosdep \ && rm -rf /var/lib/apt/lists/* -# Install OpenCV---------------------------------------------------------------- -# Install depending packages -RUN apt-get update && apt-get install -y \ - build-essential \ - cmake \ - git \ - pkg-config \ - libgtk-3-dev \ - libavcodec-dev \ - libavformat-dev \ - libswscale-dev \ - libv4l-dev \ - libxvidcore-dev \ - libx264-dev \ - libjpeg-dev \ - libpng-dev \ - libtiff-dev \ - gfortran \ - openexr \ - libatlas-base-dev \ - python3-dev \ - python3-numpy \ - libtbb2 \ - libtbb-dev \ - libdc1394-22-dev \ - && rm -rf /var/lib/apt/lists/* - -# Downloading OpenCV repo & switching to 3.3.1 branch, -# Building OpenCV -RUN mkdir /opencv_build && cd /opencv_build && \ - git clone https://github.com/opencv/opencv.git && \ - cd opencv && git checkout 3.3.1 && cd .. && \ - git clone https://github.com/opencv/opencv_contrib.git && \ - cd opencv_contrib && git checkout 3.3.1 && \ - cd /opencv_build/opencv && mkdir build && cd build && \ - cmake -D CMAKE_BUILD_TYPE=RELEASE \ - -D CMAKE_INSTALL_PREFIX=${install_path:-/usr/local} \ - -D INSTALL_C_EXAMPLES=ON \ - -D INSTALL_PYTHON_EXAMPLES=ON \ - -D OPENCV_GENERATE_PKGCONFIG=ON \ - -D OPENCV_EXTRA_MODULES_PATH=/opencv_build/opencv_contrib/modules \ - -D BUILD_EXAMPLES=ON \ - -D OPENCV_ENABLED_NONFREE=YES \ - -D ENABLE_PRECOMPILED_HEADERS=OFF .. && \ - make -j6 -RUN cd /opencv_build/opencv/build \ - && make install \ - && rm -rf /opencv_build \ - && rm -rf /var/lib/apt/lists/* - # Install Astrobee---------------------------------------------------------------- COPY ./scripts/setup/debians /setup/astrobee/debians RUN apt-get update \ - && /setup/astrobee/install_luajit.sh \ - && /setup/astrobee/debians/build_install_debians_18_04.sh \ + && /setup/astrobee/debians/build_install_debians.sh \ && rm -rf /var/lib/apt/lists/* COPY ./scripts/setup/packages_*.lst /setup/astrobee/ # note apt-get update is run within the following shell script -RUN /setup/astrobee/install_desktop_18_04_packages.sh \ +RUN /setup/astrobee/install_desktop_packages.sh \ && rm -rf /var/lib/apt/lists/* #Add new sudo user diff --git a/scripts/docker/astrobee_melodic.Dockerfile b/scripts/docker/astrobee_melodic.Dockerfile index 71a451551b..e36fe3b860 100755 --- a/scripts/docker/astrobee_melodic.Dockerfile +++ b/scripts/docker/astrobee_melodic.Dockerfile @@ -4,7 +4,6 @@ FROM astrobee/astrobee:base-latest-melodic -RUN /setup/astrobee/install_luajit.sh COPY . /src/astrobee RUN /src/astrobee/scripts/configure.sh -l -F -D -T -p /opt/astrobee -b /build/astrobee RUN cd /build/astrobee && make -j`nproc` diff --git a/scripts/docker/build.sh b/scripts/docker/build.sh index 8a7ec6d0e7..04ef4436ae 100755 --- a/scripts/docker/build.sh +++ b/scripts/docker/build.sh @@ -16,6 +16,7 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +set -e # short help usage_string="$scriptname [-h] [-n ]" diff --git a/scripts/docker/cross_compile/astrobee_cross.Dockerfile b/scripts/docker/cross_compile/astrobee_cross.Dockerfile index eda0e081e0..dc0a0efd5c 100644 --- a/scripts/docker/cross_compile/astrobee_cross.Dockerfile +++ b/scripts/docker/cross_compile/astrobee_cross.Dockerfile @@ -28,5 +28,6 @@ ARG ARMHF_CHROOT_DIR=/arm_cross/rootfs ARG ARMHF_TOOLCHAIN=/arm_cross/toolchain/gcc # Cross-compile +RUN ln -s /arm_cross/rootfs/usr/include/eigen3 /usr/include/eigen3 RUN ./src/astrobee/scripts/configure.sh -a -p /opt/astrobee -b /build RUN cd /build && make -j4 install diff --git a/scripts/docker/cross_compile/cross_compile.sh b/scripts/docker/cross_compile/cross_compile.sh index 045489e138..164afbaeae 100755 --- a/scripts/docker/cross_compile/cross_compile.sh +++ b/scripts/docker/cross_compile/cross_compile.sh @@ -25,6 +25,7 @@ # Base docker image copies over the toolchain and rootfs, resulting from # the NASA_INSTALL.md instructions. Also installs minimum dependencies +set -e # Check that the paths are defined DIR=$(dirname "$(readlink -f "$0")") diff --git a/scripts/docker/cross_compile/debian_compile.sh b/scripts/docker/cross_compile/debian_compile.sh index 0b2393e0d2..60998d082e 100755 --- a/scripts/docker/cross_compile/debian_compile.sh +++ b/scripts/docker/cross_compile/debian_compile.sh @@ -25,6 +25,7 @@ # Base docker image copies over the toolchain and rootfs, resulting from # the NASA_INSTALL.md instructions. Also installs minimum dependencies +set -e # Check that the paths are defined DIR=$(dirname "$(readlink -f "$0")") diff --git a/scripts/git/cpplint.py b/scripts/git/cpplint.py index 9dfdbb581c..1b04b2946b 100755 --- a/scripts/git/cpplint.py +++ b/scripts/git/cpplint.py @@ -456,7 +456,7 @@ # False positives include C-style multi-line comments and multi-line strings # but those have always been troublesome for cpplint. _ALT_TOKEN_REPLACEMENT_PATTERN = re.compile( - r'[ =()](' + ('|'.join(_ALT_TOKEN_REPLACEMENT.keys())) + r')(?=[ (]|$)') + r'[ =()](' + ('|'.join(list(_ALT_TOKEN_REPLACEMENT.keys()))) + r')(?=[ (]|$)') # These constants define types of headers for use with @@ -836,7 +836,7 @@ def IncrementErrorCount(self, category): def PrintErrorCounts(self): """Print a summary of errors by category, and the total.""" - for category, count in self.errors_by_category.iteritems(): + for category, count in self.errors_by_category.items(): sys.stderr.write('Category \'%s\' errors found: %d\n' % (category, count)) sys.stderr.write('Total errors found: %d\n' % self.error_count) @@ -1392,7 +1392,7 @@ def FindEndOfExpressionInLine(line, startpos, stack): On finding an unclosed expression: (-1, None) Otherwise: (-1, new stack at end of this line) """ - for i in xrange(startpos, len(line)): + for i in range(startpos, len(line)): char = line[i] if char in '([{': # Found start of parenthesized expression, push to expression stack @@ -1621,7 +1621,7 @@ def CheckForCopyright(filename, lines, error): # We'll say it should occur by line 10. Don't forget there's a # dummy line at the front. - for line in xrange(1, min(len(lines), 11)): + for line in range(1, min(len(lines), 11)): if re.search(r'Copyright', lines[line], re.I): break else: # means no copyright line was found error(filename, 0, 'legal/copyright', 5, @@ -1772,7 +1772,7 @@ def CheckForBadCharacters(filename, lines, error): error: The function to call with any errors found. """ for linenum, line in enumerate(lines): - if u'\ufffd' in line: + if '\ufffd' in line: error(filename, linenum, 'readability/utf8', 5, 'Line contains invalid UTF-8 (or Unicode replacement character).') if '\0' in line: @@ -2818,7 +2818,7 @@ def CheckForFunctionLengths(filename, clean_lines, linenum, if starting_func: body_found = False - for start_linenum in xrange(linenum, clean_lines.NumLines()): + for start_linenum in range(linenum, clean_lines.NumLines()): start_line = lines[start_linenum] joined_line += ' ' + start_line.lstrip() if Search(r'(;|})', start_line): # Declarations and trivial functions @@ -3295,7 +3295,7 @@ def CheckBracesSpacing(filename, clean_lines, linenum, error): trailing_text = '' if endpos > -1: trailing_text = endline[endpos:] - for offset in xrange(endlinenum + 1, + for offset in range(endlinenum + 1, min(endlinenum + 3, clean_lines.NumLines() - 1)): trailing_text += clean_lines.elided[offset] if not Match(r'^[\s}]*[{.;,)<>\]:]', trailing_text): @@ -3464,7 +3464,7 @@ def IsRValueType(clean_lines, nesting_state, linenum, column): # Look for the previous 'for(' in the previous lines. before_text = match_symbol.group(1) - for i in xrange(start - 1, max(start - 6, 0), -1): + for i in range(start - 1, max(start - 6, 0), -1): before_text = clean_lines.elided[i] + before_text if Search(r'for\s*\([^{};]*$', before_text): # This is the condition inside a for-loop @@ -3591,12 +3591,12 @@ def IsRValueAllowed(clean_lines, linenum): True if line is within the region where RValue references are allowed. """ # Allow region marked by PUSH/POP macros - for i in xrange(linenum, 0, -1): + for i in range(linenum, 0, -1): line = clean_lines.elided[i] if Match(r'GOOGLE_ALLOW_RVALUE_REFERENCES_(?:PUSH|POP)', line): if not line.endswith('PUSH'): return False - for j in xrange(linenum, clean_lines.NumLines(), 1): + for j in range(linenum, clean_lines.NumLines(), 1): line = clean_lines.elided[j] if Match(r'GOOGLE_ALLOW_RVALUE_REFERENCES_(?:PUSH|POP)', line): return line.endswith('POP') @@ -4076,7 +4076,7 @@ def CheckCheck(filename, clean_lines, linenum, error): expression = lines[linenum][start_pos + 1:end_pos - 1] else: expression = lines[linenum][start_pos + 1:] - for i in xrange(linenum + 1, end_line): + for i in range(linenum + 1, end_line): expression += lines[i] expression += last_line[0:end_pos - 1] @@ -4204,7 +4204,7 @@ def GetLineWidth(line): The width of the line in column positions, accounting for Unicode combining characters and wide characters. """ - if isinstance(line, unicode): + if isinstance(line, str): width = 0 for uc in unicodedata.normalize('NFC', line): if unicodedata.east_asian_width(uc) in ('W', 'F'): @@ -4557,7 +4557,7 @@ def _GetTextInside(text, start_pattern): # Give opening punctuations to get the matching close-punctuations. matching_punctuation = {'(': ')', '{': '}', '[': ']'} - closing_punctuation = set(matching_punctuation.itervalues()) + closing_punctuation = set(matching_punctuation.values()) # Find the position to start extracting text. match = re.search(start_pattern, text, re.M) @@ -4883,7 +4883,7 @@ def IsDerivedFunction(clean_lines, linenum): virt-specifier. """ # Scan back a few lines for start of current function - for i in xrange(linenum, max(-1, linenum - 10), -1): + for i in range(linenum, max(-1, linenum - 10), -1): match = Match(r'^([^()]*\w+)\(', clean_lines.elided[i]) if match: # Look for "override" after the matching closing parenthesis @@ -4904,7 +4904,7 @@ def IsInitializerList(clean_lines, linenum): True if current line appears to be inside constructor initializer list, False otherwise. """ - for i in xrange(linenum, 1, -1): + for i in range(linenum, 1, -1): line = clean_lines.elided[i] if i == linenum: remove_function_body = Match(r'^(.*)\{\s*$', line) @@ -5000,7 +5000,7 @@ def CheckForNonConstReference(filename, clean_lines, linenum, # Found the matching < on an earlier line, collect all # pieces up to current line. line = '' - for i in xrange(startline, linenum + 1): + for i in range(startline, linenum + 1): line += clean_lines.elided[i].strip() # Check for non-const references in function parameters. A single '&' may @@ -5024,7 +5024,7 @@ def CheckForNonConstReference(filename, clean_lines, linenum, # appear inside the second set of parentheses on the current line as # opposed to the first set. if linenum > 0: - for i in xrange(linenum - 1, max(0, linenum - 10), -1): + for i in range(linenum - 1, max(0, linenum - 10), -1): previous_line = clean_lines.elided[i] if not Search(r'[),]\s*$', previous_line): break @@ -5055,7 +5055,7 @@ def CheckForNonConstReference(filename, clean_lines, linenum, # Don't see a whitelisted function on this line. Actually we # didn't see any function name on this line, so this is likely a # multi-line parameter list. Try a bit harder to catch this case. - for i in xrange(2): + for i in range(2): if (linenum > i and Search(whitelisted_functions, clean_lines.elided[linenum - i - 1])): return @@ -5217,7 +5217,7 @@ def CheckCStyleCast(filename, clean_lines, linenum, cast_type, pattern, error): # Try expanding current context to see if we one level of # parentheses inside a macro. if linenum > 0: - for i in xrange(linenum - 1, max(0, linenum - 5), -1): + for i in range(linenum - 1, max(0, linenum - 5), -1): context = clean_lines.elided[i] + context if Match(r'.*\b[_A-Z][_A-Z0-9]*\s*\((?:\([^()]*\)|[^()])*$', context): return False @@ -5474,7 +5474,7 @@ def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error, required = {} # A map of header name to linenumber and the template entity. # Example of required: { '': (1219, 'less<>') } - for linenum in xrange(clean_lines.NumLines()): + for linenum in range(clean_lines.NumLines()): line = clean_lines.elided[linenum] if not line or line[0] == '#': continue @@ -5523,7 +5523,7 @@ def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error, # include_dict is modified during iteration, so we iterate over a copy of # the keys. - header_keys = include_dict.keys() + header_keys = list(include_dict.keys()) for header in header_keys: (same_module, common_path) = FilesBelongToSameModule(abs_filename, header) fullpath = common_path + header @@ -5618,7 +5618,7 @@ def CheckRedundantVirtual(filename, clean_lines, linenum, error): end_col = -1 end_line = -1 start_col = len(virtual.group(1)) - for start_line in xrange(linenum, min(linenum + 3, clean_lines.NumLines())): + for start_line in range(linenum, min(linenum + 3, clean_lines.NumLines())): line = clean_lines.elided[start_line][start_col:] parameter_list = Match(r'^([^(]*)\(', line) if parameter_list: @@ -5633,7 +5633,7 @@ def CheckRedundantVirtual(filename, clean_lines, linenum, error): # Look for "override" or "final" after the parameter list # (possibly on the next few lines). - for i in xrange(end_line, min(end_line + 3, clean_lines.NumLines())): + for i in range(end_line, min(end_line + 3, clean_lines.NumLines())): line = clean_lines.elided[i][end_col:] match = Search(r'\b(override|final)\b', line) if match: @@ -5852,7 +5852,7 @@ def ProcessFileData(filename, file_extension, lines, error, RemoveMultiLineComments(filename, lines, error) clean_lines = CleansedLines(lines) - for line in xrange(clean_lines.NumLines()): + for line in range(clean_lines.NumLines()): ProcessLine(filename, file_extension, clean_lines, line, include_state, function_state, nesting_state, error, extra_check_functions) diff --git a/scripts/git/cpplint_repo.py b/scripts/git/cpplint_repo.py index b5fb39f4fa..98c95144a6 100755 --- a/scripts/git/cpplint_repo.py +++ b/scripts/git/cpplint_repo.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os, imp, fnmatch @@ -25,12 +25,12 @@ def run_cpplint(filename, cpplint_path): return cpplint.output def print_objection(): - print " ____ __ _ __ _ __" - print " / __ \/ /_ (_)__ _____/ /_(_)___ ____ / /" - print " / / / / __ \ / / _ \/ ___/ __/ / __ \/ __ \/ / " - print "/ /_/ / /_/ / / / __/ /__/ /_/ / /_/ / / / /_/ " - print "\____/_.___/_/ /\___/\___/\__/_/\____/_/ /_(_) " - print " /___/ " + print(" ____ __ _ __ _ __") + print(" / __ \/ /_ (_)__ _____/ /_(_)___ ____ / /") + print(" / / / / __ \ / / _ \/ ___/ __/ / __ \/ __ \/ / ") + print("/ /_/ / /_/ / / / __/ /__/ /_/ / /_/ / / / /_/ ") + print("\____/_.___/_/ /\___/\___/\__/_/\____/_/ /_(_) ") + print(" /___/ ") def main(): num_errors = 0 @@ -41,7 +41,7 @@ def main(): # Lets look for source files and headers in our repo for root, dirnames, filenames in os.walk(get_repo_path()): if "Software" in root or "external" in root or "gnc/matlab" in root or \ - "submodules" in root: + "submodules" in root or "debian" in root or "build" in root: continue for filename in filenames: if "agast_score" in filename or "brisk" in filename: @@ -56,13 +56,13 @@ def main(): num_errors += len(output) for error in output: - print "%s:%s: %s" % ((root + "/" + filename).replace(get_repo_path() + "/", ''), str(error[0]), error[1]) + print("%s:%s: %s" % ((root + "/" + filename).replace(get_repo_path() + "/", ''), str(error[0]), error[1])) - print "="*50 + print("="*50) if num_errors > 0: - print " You have %d lint errors" % num_errors + print(" You have %d lint errors" % num_errors) elif num_errors == 0: - print " Code adheres to style guide lines" + print(" Code adheres to style guide lines") exit(num_errors) diff --git a/scripts/git/pre-commit b/scripts/git/pre-commit index c535283ff2..16a70e762d 100755 --- a/scripts/git/pre-commit +++ b/scripts/git/pre-commit @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import datetime, os, imp, subprocess from operator import itemgetter @@ -11,7 +11,7 @@ def get_cpplint_path(): root = pwd.split("/").index("FreeFlyerMLP") return "/".join(pwd.split("/")[:root+1] + ["scripts","git","cpplint.py"]) else: - return get_repo_path() + "/scripts/git/cpplint.py" + return get_repo_path().decode() + "/scripts/git/cpplint.py" return "" def get_repo_path(): @@ -22,10 +22,10 @@ def get_repo_path(): return "/".join(pwd.split("/")[:root+1]) else: try: - return subprocess.Popen(["git", "rev-parse", "--show-toplevel"], - stdout=subprocess.PIPE).communicate()[0].rstrip() + return subprocess.Popen(["git", "rev-parse", "--show-toplevel"], + stdout=subprocess.PIPE).communicate()[0].rstrip() except: - print "Can't find repo path" + print("Can't find repo path") exit(1) return "" @@ -44,12 +44,12 @@ def run_cpplint(filename, cpplint_path): return cpplint.output def print_objection(): - print " ____ __ _ __ _ __" - print " / __ \/ /_ (_)__ _____/ /_(_)___ ____ / /" - print " / / / / __ \ / / _ \/ ___/ __/ / __ \/ __ \/ / " - print "/ /_/ / /_/ / / / __/ /__/ /_/ / /_/ / / / /_/ " - print "\____/_.___/_/ /\___/\___/\__/_/\____/_/ /_(_) " - print " /___/ " + print(" ____ __ _ __ _ __") + print(" / __ \/ /_ (_)__ _____/ /_(_)___ ____ / /") + print(" / / / / __ \ / / _ \/ ___/ __/ / __ \/ __ \/ / ") + print("/ /_/ / /_/ / / / __/ /__/ /_/ / /_/ / / / /_/ ") + print("\____/_.___/_/ /\___/\___/\__/_/\____/_/ /_(_) ") + print(" /___/ ") def main(): num_errors = 0 @@ -63,15 +63,16 @@ def main(): stdout=subprocess.PIPE, stderr=subprocess.PIPE) output, err = p.communicate() if p.returncode: - print "Failed to run git ls-files" + print("Failed to run git ls-files") exit(1) cpp_files_to_lint = [] for filename in output.split(): - if filename.endswith((".cpp",".cc",".h",".hpp",".cc.in",".c",".c.in",".h.in", + if str(filename).endswith((".cpp",".cc",".h",".hpp",".cc.in",".c",".c.in",".h.in", ".hpp.in",".cxx",".hxx")): if not "external/" in filename and not "Software/" in filename and \ not "gnc/matlab/" in filename and not "agast_score" in filename and \ - not "brisk" in filename: + not "brisk" in filename and not "debian" in filename and \ + not "build" in filename: cpp_files_to_lint.append(filename) for filename in cpp_files_to_lint: @@ -85,26 +86,26 @@ def main(): if len(output) > 0: lines = [] for error in output: - print "%s:%s: %s" % (filename, str(error[0]), error[1]) + print("%s:%s: %s" % (filename, str(error[0]), error[1])) lines.append(error[0]) # Run the clang-format if there are errors in the identified ranges if len(output) > 0: command = "clang-format-8 -style=file -i " + filename ranges = [] - for k, g in groupby(enumerate(lines), lambda (i,x):i-x): - group = map(itemgetter(1), g) + for k, g in groupby(enumerate(lines), lambda i_x:i_x[0]-i_x[1]): + group = list(map(itemgetter(1), g)) ranges.append((group[0], group[-1])) for clang_range in ranges: command = command + " --lines=" + str(clang_range[0]) + ":" + str(clang_range[1]) # print command retcode = subprocess.Popen(command, shell=True) - print "="*50 + print("="*50) if num_errors > 0: - print " You have %d lint errors, commit failed" % num_errors - print " The errors were automatically piped through clang-format-8" + print(" You have %d lint errors, commit failed" % num_errors) + print(" The errors were automatically piped through clang-format-8") elif num_errors == 0: - print " Code adheres to style guide lines, committing ..." + print(" Code adheres to style guide lines, committing ...") exit(num_errors) diff --git a/scripts/setup/debians/.gitignore b/scripts/setup/debians/.gitignore index 583ec2c799..8c71842862 100644 --- a/scripts/setup/debians/.gitignore +++ b/scripts/setup/debians/.gitignore @@ -15,3 +15,6 @@ libdbowdlib libopenmvg libdecomputil libjps3d +libmiro +libsoracore +luajit-2.0 diff --git a/scripts/setup/debians/alvar/changelog b/scripts/setup/debians/alvar/changelog index 118ea478b8..53e734b73e 100644 --- a/scripts/setup/debians/alvar/changelog +++ b/scripts/setup/debians/alvar/changelog @@ -2,13 +2,13 @@ libalvar (2.0-4) unstable; urgency=medium * Ros updated opencv, changed path, needs updated rpath. - -- Brian Coltin Mon, 30 Apr 2018 13:05:46 -0700 + -- Brian Coltin Mon, 30 Apr 2018 13:05:46 -0700 libalvar (2.0-3) unstable; urgency=medium * Bump ros-kinetic-opencv version. - -- Brian Coltin Mon, 11 Dec 2017 17:35:08 -0800 + -- Brian Coltin Mon, 11 Dec 2017 17:35:08 -0800 libalvar (2.0-2) unstable; urgency=medium diff --git a/scripts/setup/debians/build_install_debians.sh b/scripts/setup/debians/build_install_debians.sh index dac56ba6e4..9fab37fe51 100755 --- a/scripts/setup/debians/build_install_debians.sh +++ b/scripts/setup/debians/build_install_debians.sh @@ -27,42 +27,82 @@ sudo apt-get install -y devscripts equivs libproj-dev # delete old debians (-f avoids 'no such file' warning on first run) rm -f *_amd64.deb +DIST=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` + +if [ "$DIST" = "xenial" ]; then + echo "Ubuntu 16 detected" +elif [ "$DIST" = "$bionic" ]; then + echo "Ubuntu 18 detected" + # Install dependencies + ${DEBIAN_LOC}/install_opencv.sh + ${DEBIAN_LOC}/install_luajit.sh + + # alvar + cp ${DEBIAN_LOC}/files_18_04/alvar_rules ${DEBIAN_LOC}/alvar/rules + cp ${DEBIAN_LOC}/files_18_04/alvar_control ${DEBIAN_LOC}/alvar/control + cp ${DEBIAN_LOC}/files_18_04/alvar_changelog ${DEBIAN_LOC}/alvar/changelog + # dlib + cp ${DEBIAN_LOC}/files_18_04/dlib_rules ${DEBIAN_LOC}/dlib/rules + cp ${DEBIAN_LOC}/files_18_04/dlib_control ${DEBIAN_LOC}/dlib/control + cp ${DEBIAN_LOC}/files_18_04/dlib_changelog ${DEBIAN_LOC}/dlib/changelog + # dbow2 + cp ${DEBIAN_LOC}/files_18_04/dbow2_rules ${DEBIAN_LOC}/dbow2/rules + cp ${DEBIAN_LOC}/files_18_04/dbow2_control ${DEBIAN_LOC}/dbow2/control + cp ${DEBIAN_LOC}/files_18_04/dbow2_changelog ${DEBIAN_LOC}/dbow2/changelog + # gtsam + cp ${DEBIAN_LOC}/files_18_04/gtsam_changelog ${DEBIAN_LOC}/gtsam/changelog + # decomputil + #jps3d + sudo apt-get install -y libvtk6.3 libboost-filesystem1.62.0 libboost-system1.62.0 + cp ${DEBIAN_LOC}/files_18_04/jps3d_changelog ${DEBIAN_LOC}/jps3d/changelog + # openmvg +elif [ "$DIST" = "focal" ]; then + echo "Ubuntu 20 detected" +fi + +# alvar cd ${DEBIAN_LOC}/alvar sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_alvar.sh || exit 1 sudo dpkg -i libalvar*_amd64.deb || exit 1 +# dlib cd ${DEBIAN_LOC}/dlib sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_dlib.sh || exit 1 sudo dpkg -i libdbowdlib*_amd64.deb || exit 1 +# dbow2 cd ${DEBIAN_LOC}/dbow2 sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_dbow2.sh || exit 1 sudo dpkg -i libdbow*_amd64.deb || exit 1 +# gtsam cd ${DEBIAN_LOC}/gtsam sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_gtsam.sh || exit 1 sudo dpkg -i libgtsam*_amd64.deb || exit 1 +# decomputil cd ${DEBIAN_LOC}/decomputil sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_decomputil.sh || exit 1 sudo dpkg -i libdecomputil*_amd64.deb || exit 1 +# jps3d cd ${DEBIAN_LOC}/jps3d sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} ./build_jps3d.sh || exit 1 sudo dpkg -i libjps3d*_amd64.deb || exit 1 +# openmvg cd ${DEBIAN_LOC}/openmvg sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} diff --git a/scripts/setup/debians/build_install_debians_18_04.sh b/scripts/setup/debians/build_install_debians_18_04.sh deleted file mode 100755 index ac75038b32..0000000000 --- a/scripts/setup/debians/build_install_debians_18_04.sh +++ /dev/null @@ -1,79 +0,0 @@ -#/bin/bash -e -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -# -# Install the dependencies needed for the debians. Build and install flight -# software debians. - -DEBIAN_LOC=$(dirname "$(readlink -f "$0")") - -sudo apt-get install -y devscripts equivs libproj-dev - -# delete old debians (-f avoids 'no such file' warning on first run) -rm -f *_amd64.deb - -cp ${DEBIAN_LOC}/files_18_04/alvar_rules ${DEBIAN_LOC}/alvar/rules -cp ${DEBIAN_LOC}/files_18_04/alvar_control ${DEBIAN_LOC}/alvar/control -cd ${DEBIAN_LOC}/alvar -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_alvar.sh || exit 1 -sudo dpkg -i libalvar*_amd64.deb || exit 1 - -cp ${DEBIAN_LOC}/files_18_04/dlib_rules ${DEBIAN_LOC}/dlib/rules -cp ${DEBIAN_LOC}/files_18_04/dlib_control ${DEBIAN_LOC}/dlib/control -cd ${DEBIAN_LOC}/dlib -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dlib.sh || exit 1 -sudo dpkg -i libdbowdlib*_amd64.deb || exit 1 - -cp ${DEBIAN_LOC}/files_18_04/dbow2_rules ${DEBIAN_LOC}/dbow2/rules -cp ${DEBIAN_LOC}/files_18_04/dbow2_control ${DEBIAN_LOC}/dbow2/control -cd ${DEBIAN_LOC}/dbow2 -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_dbow2.sh || exit 1 -sudo dpkg -i libdbow*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/gtsam -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_gtsam.sh || exit 1 -sudo dpkg -i libgtsam*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/decomputil -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_decomputil.sh || exit 1 -sudo dpkg -i libdecomputil*_amd64.deb || exit 1 - -# install depend libraries -sudo apt-get install -y libvtk6.3 libboost-filesystem1.62.0 libboost-system1.62.0 -cd ${DEBIAN_LOC}/jps3d -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_jps3d.sh || exit 1 -sudo dpkg -i libjps3d*_amd64.deb || exit 1 - -cd ${DEBIAN_LOC}/openmvg -sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control -cd ${DEBIAN_LOC} -./build_openmvg.sh || exit 1 -sudo dpkg -i libopenmvg*_amd64.deb || exit 1 - diff --git a/scripts/setup/debians/build_miro.sh b/scripts/setup/debians/build_miro.sh new file mode 100755 index 0000000000..a85f9339a2 --- /dev/null +++ b/scripts/setup/debians/build_miro.sh @@ -0,0 +1,36 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This package depends on rti which is a proprietary package and therefore +# cannot be shared. This debian build is only useful for NASA maintenence +# of this repo. + +PACKAGE_NAME=libmiro +ORIG_TAR=libmiro_0.3.2.orig.tar.gz +DEB_DIR=miro + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet https://github.com/hhutz/Miro.git --branch catkin_build $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/build_soracore.sh b/scripts/setup/debians/build_soracore.sh new file mode 100755 index 0000000000..6eaa7b30e5 --- /dev/null +++ b/scripts/setup/debians/build_soracore.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + + +# This package depends on rti which is a proprietary package and therefore +# cannot be shared. It is not necessary for running the astrobee simulation. +# This debian build is only useful for NASA maintenence of this repo. + +PACKAGE_NAME=libsoracore +ORIG_TAR=libsoracore_2.0.orig.tar.gz +DEB_DIR=soracore + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone https://github.com/marinagmoreira/soraCore.git --branch catkin_build $PACKAGE_NAME 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/dbow2/changelog b/scripts/setup/debians/dbow2/changelog index e5428d0ca4..c7ab23b4c0 100644 --- a/scripts/setup/debians/dbow2/changelog +++ b/scripts/setup/debians/dbow2/changelog @@ -2,13 +2,13 @@ libdbow2 (0.1-5) unstable; urgency=medium * Ros moved opencv library which broke build, fixed it. - -- Brian Coltin Mon, 30 Apr 2018 13:31:16 -0700 + -- Brian Coltin Mon, 30 Apr 2018 13:31:16 -0700 libdbow2 (0.1-4) unstable; urgency=medium * Bump ros-kinetic-opencv version. - -- Brian Coltin Mon, 11 Dec 2017 17:34:43 -0800 + -- Brian Coltin Mon, 11 Dec 2017 17:34:43 -0800 libdbow2 (0.1-3) unstable; urgency=medium diff --git a/scripts/setup/debians/dlib/changelog b/scripts/setup/debians/dlib/changelog index 1f15abd86e..bc19859905 100644 --- a/scripts/setup/debians/dlib/changelog +++ b/scripts/setup/debians/dlib/changelog @@ -2,13 +2,13 @@ libdbowdlib (0.1-3) unstable; urgency=medium * Ros moved opencv library which broke build, fixed it. - -- Brian Coltin Mon, 30 Apr 2018 13:31:51 -0700 + -- Brian Coltin Mon, 30 Apr 2018 13:31:51 -0700 libdbowdlib (0.1-2) unstable; urgency=medium * Bump version number due to ros opencv upgrade. - -- Brian Coltin Mon, 11 Dec 2017 17:31:09 -0800 + -- Brian Coltin Mon, 11 Dec 2017 17:31:09 -0800 libdbowdlib (0.1-1) unstable; urgency=medium diff --git a/scripts/setup/debians/files_18_04/alvar_changelog b/scripts/setup/debians/files_18_04/alvar_changelog new file mode 100644 index 0000000000..608570b7c8 --- /dev/null +++ b/scripts/setup/debians/files_18_04/alvar_changelog @@ -0,0 +1,35 @@ +libalvar (2.0-5) unstable; urgency=medium + + * Compatible with ubuntu 18, dropped ros-opencv dependency. + + -- Brian Coltin Tue, 15 May 2021 13:05:46 -0700 + +libalvar (2.0-4) unstable; urgency=medium + + * Ros updated opencv, changed path, needs updated rpath. + + -- Brian Coltin Mon, 30 Apr 2018 13:05:46 -0700 + +libalvar (2.0-3) unstable; urgency=medium + + * Bump ros-kinetic-opencv version. + + -- Brian Coltin Mon, 11 Dec 2017 17:35:08 -0800 + +libalvar (2.0-2) unstable; urgency=medium + + * Build from original tarball now. + + -- Brian Coltin Tue, 01 Aug 2017 15:31:34 -0700 + +libalvar (2.0-1) stable; urgency=medium + + * Changed to the correct version number. + + -- Brian Coltin Fri, 05 May 2017 14:25:42 -0700 + +libalvar (0.1-1) unstable; urgency=medium + + * Initial release. + + -- Brian Coltin Wed, 18 Jan 2017 13:43:06 -0800 diff --git a/scripts/setup/debians/files_18_04/dbow2_changelog b/scripts/setup/debians/files_18_04/dbow2_changelog new file mode 100644 index 0000000000..8d036a3a87 --- /dev/null +++ b/scripts/setup/debians/files_18_04/dbow2_changelog @@ -0,0 +1,35 @@ +libdbow2 (0.1-6) unstable; urgency=medium + + * Compatible with ubuntu 18, dropped ros-opencv dependency. + + -- Brian Coltin Tue, 15 May 2021 13:05:46 -0700 + +libdbow2 (0.1-5) unstable; urgency=medium + + * Ros moved opencv library which broke build, fixed it. + + -- Brian Coltin Mon, 30 Apr 2018 13:31:16 -0700 + +libdbow2 (0.1-4) unstable; urgency=medium + + * Bump ros-kinetic-opencv version. + + -- Brian Coltin Mon, 11 Dec 2017 17:34:43 -0800 + +libdbow2 (0.1-3) unstable; urgency=medium + + * Separate dlib into separate debian. + + -- Brian Coltin Tue, 29 Aug 2017 15:30:32 -0700 + +libdbow2 (0.1-2) unstable; urgency=medium + + * Built from original tarball, not version in git. + + -- Brian Coltin Tue, 01 Aug 2017 15:29:05 -0700 + +libdbow2 (0.1-1) unstable; urgency=medium + + * Initial release. + + -- Brian Coltin Tue, 17 Jan 2017 11:21:32 -0800 diff --git a/scripts/setup/debians/files_18_04/dlib_changelog b/scripts/setup/debians/files_18_04/dlib_changelog new file mode 100644 index 0000000000..c3eb8597ff --- /dev/null +++ b/scripts/setup/debians/files_18_04/dlib_changelog @@ -0,0 +1,23 @@ +libdbowdlib (0.1-4) unstable; urgency=medium + + * Compatible with ubuntu 18, dropped ros-opencv dependency. + + -- Brian Coltin Tue, 15 May 2021 13:05:46 -0700 + +libdbowdlib (0.1-3) unstable; urgency=medium + + * Ros moved opencv library which broke build, fixed it. + + -- Brian Coltin Mon, 30 Apr 2018 13:31:51 -0700 + +libdbowdlib (0.1-2) unstable; urgency=medium + + * Bump version number due to ros opencv upgrade. + + -- Brian Coltin Mon, 11 Dec 2017 17:31:09 -0800 + +libdbowdlib (0.1-1) unstable; urgency=medium + + * Initial release. + + -- Brian Coltin Tue, 29 Aug 2017 11:42:12 -0800 diff --git a/scripts/setup/debians/files_18_04/gtsam_changelog b/scripts/setup/debians/files_18_04/gtsam_changelog new file mode 100644 index 0000000000..ec60b46b42 --- /dev/null +++ b/scripts/setup/debians/files_18_04/gtsam_changelog @@ -0,0 +1,11 @@ +libgtsam (4.1.0-2) unstable; urgency=medium + + * Compatible with ubuntu 18. + + -- Ryan Soussan Tue, 15 May 2021 13:05:46 -0700 + +libgtsam (4.1.0-1) unstable; urgency=medium + + * Initial release. + + -- Ryan Soussan Mon, 31 Aug 2020 11:21:32 -0800 diff --git a/scripts/setup/debians/files_18_04/jps3d_changelog b/scripts/setup/debians/files_18_04/jps3d_changelog new file mode 100644 index 0000000000..37b767183e --- /dev/null +++ b/scripts/setup/debians/files_18_04/jps3d_changelog @@ -0,0 +1,11 @@ +libjps3d (0.1-2) unstable; urgency=medium + + * Compatible with ubuntu 18, upgraded dependencies. + + -- Brian Coltin Tue, 15 May 2021 13:05:46 -0700 + +libjps3d (0.1-1) unstable; urgency=medium + + * Initial release (Closes: #nnnn) + + -- Brian Coltin Mon, 12 Jun 2017 14:40:30 -0700 diff --git a/scripts/setup/install_luajit.sh b/scripts/setup/debians/install_luajit.sh similarity index 84% rename from scripts/setup/install_luajit.sh rename to scripts/setup/debians/install_luajit.sh index 96e8d25523..934fb8004f 100755 --- a/scripts/setup/install_luajit.sh +++ b/scripts/setup/debians/install_luajit.sh @@ -19,13 +19,14 @@ CURRENT_LOC=$(dirname "$(readlink -f "$0")") echo ${CURRENT_LOC} -# cd /usr/local/lib +cd ${CURRENT_LOC} PACKAGE_NAME=luajit-2.0 if [ -d $PACKAGE_NAME ]; then - sudo rm -rf $PACKAGE_NAME + rm -rf $PACKAGE_NAME fi -sudo git clone --quiet https://luajit.org/git/luajit-2.0.git $PACKAGE_NAME 2>&1 || exit 1 +git clone --quiet https://luajit.org/git/luajit-2.0.git $PACKAGE_NAME 2>&1 || exit 1 cd $PACKAGE_NAME -make && sudo make install +make +sudo make install # cd ${CURRENT_LOC} diff --git a/scripts/setup/install_opencv.sh b/scripts/setup/debians/install_opencv.sh similarity index 97% rename from scripts/setup/install_opencv.sh rename to scripts/setup/debians/install_opencv.sh index 18c94fa67a..43a807e1fe 100755 --- a/scripts/setup/install_opencv.sh +++ b/scripts/setup/debians/install_opencv.sh @@ -43,7 +43,7 @@ done echo "Installing in: "${install_path:-/usr/local} echo "Install the required dependencies" -sudo apt install -y build-essential cmake git pkg-config libgtk-3-dev \ +sudo apt-get install -y build-essential cmake git pkg-config libgtk-3-dev \ libavcodec-dev libavformat-dev libswscale-dev libv4l-dev \ libxvidcore-dev libx264-dev libjpeg-dev libpng-dev libtiff-dev \ gfortran openexr libatlas-base-dev python3-dev python3-numpy \ diff --git a/scripts/setup/debians/miro/README.Debian b/scripts/setup/debians/miro/README.Debian new file mode 100644 index 0000000000..6bad7a7f1f --- /dev/null +++ b/scripts/setup/debians/miro/README.Debian @@ -0,0 +1,6 @@ +libmiro for Debian +----------------- + +Created initial version. + + -- Brian Coltin Mon, 08 May 2017 12:55:37 -0700 diff --git a/scripts/setup/debians/miro/changelog b/scripts/setup/debians/miro/changelog new file mode 100644 index 0000000000..176e9c103c --- /dev/null +++ b/scripts/setup/debians/miro/changelog @@ -0,0 +1,11 @@ +libmiro (0.3.2-1) unstable; urgency=medium + + * Fix compile error in ubuntu 18.04 + + -- Brian Coltin Mon, 04 May 2021 11:45:00 -0700 + +libmiro (0.3.1-1) unstable; urgency=medium + + * Initial release (Closes: #nnnn) + + -- Brian Coltin Mon, 08 May 2017 12:55:37 -0700 diff --git a/scripts/setup/debians/miro/compat b/scripts/setup/debians/miro/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/miro/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/miro/control b/scripts/setup/debians/miro/control new file mode 100644 index 0000000000..7b31943b26 --- /dev/null +++ b/scripts/setup/debians/miro/control @@ -0,0 +1,20 @@ +Source: libmiro +Priority: optional +Maintainer: Brian Coltin +Build-Depends: debhelper (>=9), cmake (>=3.5), rti-dev (>=5.1), libqt4-qt3support (>=4), libace-dev (>=6.3) +Standards-Version: 3.9.7 +Section: libs +Homepage: https://github.com/hhutz/Miro + +Package: libmiro-dev +Section: libdevel +Architecture: any +Depends: libmiro0 (= ${binary:Version}), ${misc:Depends} +Description: Middleware for Robots + Middleware for Robots. + +Package: libmiro0 +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} +Description: Middleware for Robots. + Middleware for Robots. diff --git a/scripts/setup/debians/miro/copyright b/scripts/setup/debians/miro/copyright new file mode 100644 index 0000000000..03297673a9 --- /dev/null +++ b/scripts/setup/debians/miro/copyright @@ -0,0 +1,40 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: miro +Source: https://github.com/hhutz/Miro + +Files: * +Copyright: 2017 MIRO Authors +License: LGPL + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + . + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + . + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + +Files: debian/* +Copyright: 2017 Brian Coltin +License: GPL-2+ + This package is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + . + This package is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + . + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + On Debian systems, the complete text of the GNU General + Public License version 2 can be found in "/usr/share/common-licenses/GPL-2". + diff --git a/scripts/setup/debians/miro/libmiro-dev.debhelper.log b/scripts/setup/debians/miro/libmiro-dev.debhelper.log new file mode 100644 index 0000000000..c9ed471d30 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.debhelper.log @@ -0,0 +1,2 @@ +dh_update_autotools_config +dh_auto_configure diff --git a/scripts/setup/debians/miro/libmiro-dev.dirs b/scripts/setup/debians/miro/libmiro-dev.dirs new file mode 100644 index 0000000000..d8226e61e7 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.dirs @@ -0,0 +1,3 @@ +usr/lib +usr/lib/cmake +usr/include diff --git a/scripts/setup/debians/miro/libmiro-dev.install b/scripts/setup/debians/miro/libmiro-dev.install new file mode 100644 index 0000000000..f8e7cff6b1 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro-dev.install @@ -0,0 +1,3 @@ +usr/include/* +usr/lib/lib*.so +usr/lib/cmake/* diff --git a/scripts/setup/debians/miro/libmiro0.debhelper.log b/scripts/setup/debians/miro/libmiro0.debhelper.log new file mode 100644 index 0000000000..c9ed471d30 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.debhelper.log @@ -0,0 +1,2 @@ +dh_update_autotools_config +dh_auto_configure diff --git a/scripts/setup/debians/miro/libmiro0.dirs b/scripts/setup/debians/miro/libmiro0.dirs new file mode 100644 index 0000000000..d92f1599ee --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.dirs @@ -0,0 +1,3 @@ +usr/bin +usr/etc +usr/lib diff --git a/scripts/setup/debians/miro/libmiro0.install b/scripts/setup/debians/miro/libmiro0.install new file mode 100644 index 0000000000..689294bc36 --- /dev/null +++ b/scripts/setup/debians/miro/libmiro0.install @@ -0,0 +1,3 @@ +usr/bin/* +usr/etc/* +usr/lib/lib*.so.* diff --git a/scripts/setup/debians/miro/rules b/scripts/setup/debians/miro/rules new file mode 100755 index 0000000000..eaf77f368a --- /dev/null +++ b/scripts/setup/debians/miro/rules @@ -0,0 +1,25 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +export DEB_LDFLAGS_MAINT_APPEND = -Wl,--no-as-needed -Wl,-allow-multiple-definition + + +%: + dh $@ --buildsystem=cmake + + +# dh_make generated override targets +# This is example for Cmake (See https://bugs.debian.org/641051 ) +#override_dh_auto_configure: +# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) + diff --git a/scripts/setup/debians/miro/source/format b/scripts/setup/debians/miro/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/miro/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/debians/mirror/create_repo.sh b/scripts/setup/debians/mirror/create_repo.sh index 290c7f2998..ad94039f58 100755 --- a/scripts/setup/debians/mirror/create_repo.sh +++ b/scripts/setup/debians/mirror/create_repo.sh @@ -29,3 +29,4 @@ $APTLY mirror create -architectures=armhf xenial-security http://ports.ubuntu.co $APTLY mirror create -architectures=armhf ros http://packages.ros.org/ros/ubuntu xenial main $APTLY repo create astrobee +$APTLY repo create bionic-astrobee diff --git a/scripts/setup/debians/mirror/publish_snapshot.sh b/scripts/setup/debians/mirror/publish_snapshot.sh index 2b494e9e54..7541c59987 100755 --- a/scripts/setup/debians/mirror/publish_snapshot.sh +++ b/scripts/setup/debians/mirror/publish_snapshot.sh @@ -21,15 +21,20 @@ $APTLY snapshot create ros_$1 from mirror ros || exit $APTLY snapshot merge -latest $1 astrobee_$1 main_$1 security_$1 updates_$1 ros_$1 || exit +$APTLY snapshot create bionic-astrobee_$1 from repo bionic-astrobee || exit + # need to delete old version first $APTLY publish drop xenial +$APTLY publish drop bionic -$APTLY publish snapshot -distribution="xenial" $1 +$APTLY publish snapshot -gpg-key=33C0A17A -distribution="xenial" $1 || exit 0 +$APTLY publish snapshot -gpg-key=33C0A17A -distribution="bionic" bionic-astrobee_$1 || exit 1 PUBLISH_DIR=`$APTLY config show | grep "rootDir" | sed -r 's|^[^:]+: "([^"]+)",$|\1|'`/public chmod g+w -R --silent $PUBLISH_DIR +kinit || exit 1 rsync -ah --no-t --delete $PUBLISH_DIR/dists/ astrobee.ndc.nasa.gov:/home/irg/software/dists rsync -ah --no-t --delete $PUBLISH_DIR/pool/ astrobee.ndc.nasa.gov:/home/irg/software/pool diff --git a/scripts/setup/debians/mirror/update_astrobee_debians.sh b/scripts/setup/debians/mirror/update_astrobee_debians.sh index acfafd6b9e..408ad3dba6 100755 --- a/scripts/setup/debians/mirror/update_astrobee_debians.sh +++ b/scripts/setup/debians/mirror/update_astrobee_debians.sh @@ -4,8 +4,11 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" APTLY="$DIR/aptly/aptly -config=$DIR/aptly.conf" DEBLOC=${ASTROBEE_DEBIAN_DIR:-/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/xenial/main} +BIONIC_DEBLOC=${ASTROBEE_BIONIC_DEBIAN_DIR:-/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/bionic} # add our debians $APTLY repo add astrobee $DEBLOC/binary-armhf/*.deb $APTLY repo add astrobee $DEBLOC/binary-amd64/*.deb $APTLY repo add astrobee $DEBLOC/source/*.dsc +$APTLY repo add bionic-astrobee $BIONIC_DEBLOC/*.deb +$APTLY repo add bionic-astrobee $BIONIC_DEBLOC/*.dsc diff --git a/scripts/setup/debians/soracore/changelog b/scripts/setup/debians/soracore/changelog new file mode 100644 index 0000000000..61d89f836f --- /dev/null +++ b/scripts/setup/debians/soracore/changelog @@ -0,0 +1,11 @@ +libsoracore (2.0-1) unstable; urgency=medium + + * Ubuntu 18 compatible. + + -- Brian Coltin Tue, 11 May 2021 10:23:43 -0700 + +libsoracore (1.0-1) unstable; urgency=medium + + * Initial release. + + -- Brian Coltin Tue, 09 May 2017 10:23:43 -0700 diff --git a/scripts/setup/debians/soracore/compat b/scripts/setup/debians/soracore/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/soracore/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/soracore/control b/scripts/setup/debians/soracore/control new file mode 100644 index 0000000000..f66163383e --- /dev/null +++ b/scripts/setup/debians/soracore/control @@ -0,0 +1,25 @@ +Source: libsoracore +Priority: optional +Maintainer: Brian Coltin +Build-Depends: debhelper (>=9), rti-dev (>=5.1), libmiro-dev (>=0.3), libeigen3-dev (>=3.0) +Standards-Version: 3.9.6 +Section: libs +Homepage: https://github.com/mallanmba/soraCore/ + +Package: libsoracore-dev +Section: libdevel +Architecture: any +Depends: rti-dev (>=5.1), + libmiro-dev (>=0.3), + libeigen3-dev (>=3.0), + libsoracore1 (= ${binary:Version}), ${misc:Depends} +Description: Soracore rover software. + Rover software core libraries. + +Package: libsoracore1 +Architecture: any +Depends: rti (>=5.1), + libmiro0 (>=0.3), + ${shlibs:Depends}, ${misc:Depends} +Description: Soracore rover software. + Rover software core libraries. diff --git a/scripts/setup/debians/soracore/copyright b/scripts/setup/debians/soracore/copyright new file mode 100644 index 0000000000..da3c1edc4f --- /dev/null +++ b/scripts/setup/debians/soracore/copyright @@ -0,0 +1,37 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: libsoracore +Source: https://github.com/mallanmba/soraCore/ + +Files: * +Copyright: 2017 SoraCore Developers +License: Apache2 + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + . + http://www.apache.org/licenses/LICENSE-2.0 + . + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +Files: debian/* +Copyright: 2017 Brian Coltin +License: GPL-2+ + This package is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + . + This package is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + . + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + On Debian systems, the complete text of the GNU General + Public License version 2 can be found in "/usr/share/common-licenses/GPL-2". diff --git a/scripts/setup/debians/soracore/files b/scripts/setup/debians/soracore/files new file mode 100644 index 0000000000..ff7d04d3ae --- /dev/null +++ b/scripts/setup/debians/soracore/files @@ -0,0 +1,4 @@ +libsoracore-dev_2.0-1_amd64.deb libdevel optional +libsoracore1-dbgsym_2.0-1_amd64.ddeb debug optional +libsoracore1_2.0-1_amd64.deb libs optional +libsoracore_2.0-1_amd64.buildinfo libs optional diff --git a/scripts/setup/debians/soracore/libsoracore-dev.dirs b/scripts/setup/debians/soracore/libsoracore-dev.dirs new file mode 100644 index 0000000000..d8226e61e7 --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore-dev.dirs @@ -0,0 +1,3 @@ +usr/lib +usr/lib/cmake +usr/include diff --git a/scripts/setup/debians/soracore/libsoracore-dev.install b/scripts/setup/debians/soracore/libsoracore-dev.install new file mode 100644 index 0000000000..cbc01d537c --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore-dev.install @@ -0,0 +1,2 @@ +usr/include/* +usr/lib/cmake/* diff --git a/scripts/setup/debians/soracore/libsoracore1.dirs b/scripts/setup/debians/soracore/libsoracore1.dirs new file mode 100644 index 0000000000..8109f4f9fa --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore1.dirs @@ -0,0 +1,4 @@ +usr/lib +usr/bin +usr/etc +usr/idl diff --git a/scripts/setup/debians/soracore/libsoracore1.install b/scripts/setup/debians/soracore/libsoracore1.install new file mode 100644 index 0000000000..566eb422a6 --- /dev/null +++ b/scripts/setup/debians/soracore/libsoracore1.install @@ -0,0 +1,4 @@ +usr/lib/lib*.so +usr/bin/* +usr/etc/* +usr/idl/* diff --git a/scripts/setup/debians/soracore/patches/series b/scripts/setup/debians/soracore/patches/series new file mode 100644 index 0000000000..e69de29bb2 diff --git a/scripts/setup/debians/soracore/rules b/scripts/setup/debians/soracore/rules new file mode 100755 index 0000000000..29221f9b53 --- /dev/null +++ b/scripts/setup/debians/soracore/rules @@ -0,0 +1,23 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed + +export NDDSHOME=/opt/rti/ndds +export NDDSARCH=$(shell ls $(NDDSHOME)/lib | grep -v jdk) + +%: + dh $@ --parallel --buildsystem=cmake + +override_dh_auto_configure: + dh_auto_configure -- -DCMAKE_SHARED_LINKER_FLAGS="-Wl,-allow-multiple-definition" diff --git a/scripts/setup/debians/soracore/source/format b/scripts/setup/debians/soracore/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/soracore/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/install_desktop_16_04_packages.sh b/scripts/setup/install_desktop_16_04_packages.sh deleted file mode 100755 index a75d33aee0..0000000000 --- a/scripts/setup/install_desktop_16_04_packages.sh +++ /dev/null @@ -1,76 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -# Install a set of packages to build an Ubuntu 16.04 distro with -# all the development tools required for Astrobee work. -# -# The script uses the package list files for the list of -# packages to install. - -scriptdir=$(dirname "$(readlink -f "$0")") -arssrc=/etc/apt/sources.list.d/astrobee-latest.list - -pkg_files=${1:-$scriptdir/packages_base.lst $scriptdir/packages_desktop.lst} -echo $pkg_files - -pkgs='' -for i in $pkg_files; do - [ -r "$i" ] || continue - - # this is a non-portable bash-ism. - pkgs="$pkgs $(<"$i")" -done - -# if this file exists, we are using the astrobee repository -# need to set up tunnel through m (NASA users only) -if [ -e $arssrc ]; -then - username=${NDC_USERNAME:+${NDC_USERNAME}@} - - # Add these packages to the apt sources (we remove them later, so apt update succeeds) - - NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired - - if [ "${NO_TUNNEL}" -eq 1 ]; then - echo "Getting the custom Debian without tunnel" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software xenial main\" >> $arssrc" || exit 1 - else - echo "Tunnelling to get the custom Debian" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software xenial main\" >> $arssrc" || exit 1 - ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & - fi - - trap "kill $! 2> /dev/null; sudo truncate -s 0 $arssrc; wait $!" 0 HUP QUIT ILL ABRT FPE SEGV PIPE TERM INT - sleep 1 -fi - -sudo apt-get update || exit 1 - -if ! sudo apt-get install -m -y $pkgs; then - filter_pkgs="libroyale-dev rti-dev libsoracore-dev libmiro-dev libroyale1 rti libmiro0 libsoracore1" - for p in $filter_pkgs; do - pkgs=${pkgs//$p/} - done - sudo apt-get install -m -y $pkgs || { - echo "Couldn't install a necessary package." - exit 1 - } -fi diff --git a/scripts/setup/install_desktop_18_04_packages.sh b/scripts/setup/install_desktop_packages.sh similarity index 86% rename from scripts/setup/install_desktop_18_04_packages.sh rename to scripts/setup/install_desktop_packages.sh index ff59883f34..48fcdfcc91 100755 --- a/scripts/setup/install_desktop_18_04_packages.sh +++ b/scripts/setup/install_desktop_packages.sh @@ -25,9 +25,10 @@ scriptdir=$(dirname "$(readlink -f "$0")") arssrc=/etc/apt/sources.list.d/astrobee-latest.list +DIST=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` -pkg_files=${1:-$scriptdir/packages_base_18.lst $scriptdir/packages_desktop_18.lst} -echo $pkg_files +pkg_files=${1:-$scriptdir/packages_base_"${DIST}".lst $scriptdir/packages_desktop_${DIST}.lst} +echo "$pkg_files ${DIST}" pkgs='' for i in $pkg_files; do @@ -49,12 +50,12 @@ then if [ "${NO_TUNNEL}" -eq 1 ]; then echo "Getting the custom Debian without tunnel" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software xenial main\" >> $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb [arch=amd64] http://astrobee.ndc.nasa.gov/software ${DIST} main\" > $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb-src http://astrobee.ndc.nasa.gov/software ${DIST} main\" >> $arssrc" || exit 1 else echo "Tunnelling to get the custom Debian" - sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software xenial main\" > $arssrc" || exit 1 - sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software xenial main\" >> $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb [arch=amd64] http://127.0.0.1:8765/software ${DIST} main\" > $arssrc" || exit 1 + sudo /bin/bash -c "echo \"deb-src http://127.0.0.1:8765/software ${DIST} main\" >> $arssrc" || exit 1 ssh -N -L 8765:astrobee.ndc.nasa.gov:80 ${username}m.ndc.nasa.gov & fi diff --git a/scripts/setup/packages_base_18.lst b/scripts/setup/packages_base_bionic.lst similarity index 100% rename from scripts/setup/packages_base_18.lst rename to scripts/setup/packages_base_bionic.lst diff --git a/scripts/setup/packages_base.lst b/scripts/setup/packages_base_xenial.lst similarity index 100% rename from scripts/setup/packages_base.lst rename to scripts/setup/packages_base_xenial.lst diff --git a/scripts/setup/packages_desktop_18.lst b/scripts/setup/packages_desktop_bionic.lst similarity index 100% rename from scripts/setup/packages_desktop_18.lst rename to scripts/setup/packages_desktop_bionic.lst diff --git a/scripts/setup/packages_desktop.lst b/scripts/setup/packages_desktop_xenial.lst similarity index 100% rename from scripts/setup/packages_desktop.lst rename to scripts/setup/packages_desktop_xenial.lst diff --git a/scripts/teleop/set_data_to_disk.sh b/scripts/teleop/set_data_to_disk.sh new file mode 100755 index 0000000000..36b6a8ffe9 --- /dev/null +++ b/scripts/teleop/set_data_to_disk.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosrun executive data_to_disk_pub "$1" +echo "All done" diff --git a/shared/ff_util/include/ff_util/ff_names.h b/shared/ff_util/include/ff_util/ff_names.h index 2d2f351eb4..9fac0f788a 100644 --- a/shared/ff_util/include/ff_util/ff_names.h +++ b/shared/ff_util/include/ff_util/ff_names.h @@ -433,6 +433,7 @@ #define SERVICE_HARDWARE_LIGHT_AFT_CONTROL "hw/light_aft/control" #define SERVICE_HARDWARE_LASER_ENABLE "hw/laser/enable" #define SERVICE_HARDWARE_PMC_ENABLE "hw/pmc/enable" +#define SERVICE_HARDWARE_PMC_TIMOUT "hw/pmc/set_timeout" #define SERVICE_STREAMING_LIGHTS "hw/signal_lights/streaming" diff --git a/shared/jsonloader/include/jsonloader/command_repo.h b/shared/jsonloader/include/jsonloader/command_repo.h index 389e256029..a1ea15ff7c 100644 --- a/shared/jsonloader/include/jsonloader/command_repo.h +++ b/shared/jsonloader/include/jsonloader/command_repo.h @@ -35,8 +35,6 @@ constexpr char kCmdPause[] = "pausePlan"; constexpr char kCmdArmPanTilt[] = "armPanAndTilt"; constexpr char kCmdStationKeep[] = "stationKeep"; constexpr char kCmdWait[] = "wait"; -constexpr char kCmdClearData[] = "clearData"; -constexpr char kCmdDownload[] = "downloadData"; constexpr char kCmdPayloadOn[] = "payloadOn"; constexpr char kCmdPowerOn[] = "powerOnItem"; constexpr char kCmdPayloadOff[] = "payloadOff"; @@ -47,7 +45,6 @@ constexpr char kCmdGuestCmd[] = "guestScience"; constexpr char kCmdCustomGuest[] = "customGuestScience"; constexpr char kCmdGripper[] = "gripperControl"; constexpr char kCmdFlashlight[] = "setFlashlightBrightness"; -constexpr char kCmdGenericCmd[] = "genericCommand"; constexpr char kCmdIdleProp[] = "idlePropulsion"; constexpr char kCmdSetCamera[] = "setCamera"; constexpr char kCmdStreamCamera[] = "setCameraStreaming"; @@ -141,16 +138,6 @@ class PowerItemCommand : public Command { std::string which_; }; -class DataCommand : public Command { - public: - explicit DataCommand(Json::Value const& obj); - - std::string const& data_method() const noexcept; - - private: - std::string data_method_; -}; - class GuestScienceCommand : public Command { public: explicit GuestScienceCommand(Json::Value const& obj); @@ -185,18 +172,6 @@ class FlashlightCommand : public Command { float brightness_; }; -class GenericCommand : public Command { - public: - explicit GenericCommand(Json::Value const& obj); - - std::string const& name() const noexcept; - std::string const& param() const noexcept; - - private: - std::string name_; - std::string param_; -}; - class IdlePropulsionCommand : public Command { public: explicit IdlePropulsionCommand(Json::Value const& obj); diff --git a/shared/jsonloader/src/command_repo.cc b/shared/jsonloader/src/command_repo.cc index e2adb2953e..842f3fd770 100644 --- a/shared/jsonloader/src/command_repo.cc +++ b/shared/jsonloader/src/command_repo.cc @@ -47,14 +47,11 @@ extern const InsensitiveMap kCommandMap = { { kCmdPayloadOff, &CreateCommand }, { kCmdPowerOff, &CreateCommand }, { kCmdGripper, &CreateCommand }, - { kCmdClearData, &CreateCommand }, - { kCmdDownload, &CreateCommand }, { kCmdStartGuest, &CreateCommand }, { kCmdCustomGuest, &CreateCommand }, { kCmdGuestCmd, &CreateCommand }, { kCmdStopGuest, &CreateCommand }, { kCmdFlashlight, &CreateCommand }, - { kCmdGenericCmd, &CreateCommand }, { kCmdSetCamera, &CreateCommand }, { kCmdStreamCamera, &CreateCommand }, { kCmdRecordCamera, &CreateCommand }, @@ -105,10 +102,6 @@ const Fields powerFields { new Field("which", Json::stringValue) }; -const Fields dataFields { - new Field("dataMethod", Json::stringValue) -}; - const Fields guestScienceFields { new Field("apkName", Json::stringValue), }; @@ -123,11 +116,6 @@ const Fields flashlightFields { new Field("brightness", Json::realValue) }; -const Fields genericCommandFields { - new Field("commandName", Json::stringValue), - new Field("param", Json::stringValue) -}; - const Fields cameraFields { new EnumField("cameraName", { "Science", @@ -318,23 +306,6 @@ std::string const& PowerItemCommand::which() const noexcept { return which_; } -DataCommand::DataCommand(Json::Value const& obj) - : Command(obj), data_method_("") { - set_valid(true); - if (!Validate(obj, dataFields)) { - LOG(ERROR) << "invalid DataCommand."; - return; - } - - data_method_ = obj["dataMethod"].asString(); - - set_valid(true); -} - -std::string const& DataCommand::data_method() const noexcept { - return data_method_; -} - GuestScienceCommand::GuestScienceCommand(Json::Value const& obj) : Command(obj), apk_("") { if (!Validate(obj, guestScienceFields)) { @@ -393,27 +364,6 @@ float FlashlightCommand::brightness() const noexcept { return brightness_; } -GenericCommand::GenericCommand(Json::Value const& obj) - : Command(obj), name_(""), param_("") { - if (!Validate(obj, genericCommandFields)) { - LOG(ERROR) << "invalid GenericCommand."; - return; - } - - name_ = obj["commandName"].asString(); - param_ = obj["param"].asString(); - - set_valid(true); -} - -std::string const& GenericCommand::name() const noexcept { - return name_; -} - -std::string const& GenericCommand::param() const noexcept { - return param_; -} - IdlePropulsionCommand::IdlePropulsionCommand(Json::Value const& obj) : Command(obj) { set_valid(true); diff --git a/shared/jsonloader/test/test_command.cxx b/shared/jsonloader/test/test_command.cxx index 4525383c3a..8348df0d89 100644 --- a/shared/jsonloader/test/test_command.cxx +++ b/shared/jsonloader/test/test_command.cxx @@ -117,31 +117,6 @@ TEST(Command, VaildGripperCommand) { ASSERT_EQ(g_cmd->open(), true); } -TEST(Command, VaildDataCommand) { - const std::string data = u8R"({ - "type" : "downloadData", - "dataMethod" : "Immediate", - "blocking" : true, - "color" : "#555555", - "scopeTerminate" : true, - "name" : "3.1 DownloadData", - "id" : "0f15c5e7-dc59-457e-9cda-62394754b9de" - })"; - Json::Value v; - Json::Reader().parse(data, v, false); - - Command *cmd = Command::Make(v); - - ASSERT_NE(cmd, nullptr); - ASSERT_TRUE(cmd->valid()); - ASSERT_STREQ(cmd->type().data(), jsonloader::kCmdDownload); - - const jsonloader::DataCommand *d_cmd = - dynamic_cast(cmd); - - ASSERT_STREQ(d_cmd->data_method().data(), "Immediate"); -} - TEST(Command, ValidGuestScienceCommand) { const std::string data = u8R"({ "type" : "startGuestScience", @@ -194,32 +169,6 @@ TEST(Command, ValidGuestCustomCommand) { ASSERT_STREQ(gs_cmd->command().data(), "{name=Take, num=5, time_between=10}"); } -TEST(Command, VaildGenericCommand) { - const std::string data = u8R"({ - "type" : "genericCommand", - "commandName" : "CommandName", - "param" : "Parameters", - "blocking" : true, - "color" : "#555555", - "scopeTerminate" : true, - "name" : "0.0 GenericCommand", - "id" : "1ea5674c-802e-469f-b8a7-be5b7875b71b" - })"; - Json::Value v; - Json::Reader().parse(data, v, false); - - Command *cmd = Command::Make(v); - - ASSERT_NE(cmd, nullptr); - ASSERT_STREQ(cmd->type().data(), jsonloader::kCmdGenericCmd); - - const jsonloader::GenericCommand *g_cmd = - dynamic_cast(cmd); - - ASSERT_STREQ(g_cmd->name().data(), "CommandName"); - ASSERT_STREQ(g_cmd->param().data(), "Parameters"); -} - TEST(Command, VaildSetCameraCommand) { const std::string data = u8R"({ "type" : "setCamera", diff --git a/submodules/android b/submodules/android index ec1c90ae87..4fa00c148c 160000 --- a/submodules/android +++ b/submodules/android @@ -1 +1 @@ -Subproject commit ec1c90ae87fffb1789672cf890db25f3ce23a53b +Subproject commit 4fa00c148c514183637010cbf8a87a08beff0bbc diff --git a/submodules/platform b/submodules/platform index a7a92c4c4f..dc597f81f8 160000 --- a/submodules/platform +++ b/submodules/platform @@ -1 +1 @@ -Subproject commit a7a92c4c4ff15bd501d83243827a3eb379aea90e +Subproject commit dc597f81f8a5cf268d12cd2a350e7a6772cd0d4e diff --git a/tools/graph_bag/include/graph_bag/graph_bag.h b/tools/graph_bag/include/graph_bag/graph_bag.h index fe864daeea..cbe9d949ed 100644 --- a/tools/graph_bag/include/graph_bag/graph_bag.h +++ b/tools/graph_bag/include/graph_bag/graph_bag.h @@ -48,15 +48,7 @@ class GraphBag { private: void InitializeGraph(); void SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, - const graph_localizer::FeatureTrackMap& feature_tracks); - void SaveImuBiasTesterPredictedStates( - const std::vector& imu_bias_tester_predicted_states); - template - void SaveMsg(const MsgType& msg, const std::string& topic) { - const ros::Time timestamp = localization_common::RosTimeFromHeader(msg.header); - results_bag_.write("/" + topic, timestamp, msg); - } - + const GraphLocalizerSimulator& graph_localizer); std::unique_ptr graph_localizer_simulator_; std::unique_ptr live_measurement_simulator_; rosbag::Bag results_bag_; diff --git a/tools/graph_bag/include/graph_bag/graph_bag_params.h b/tools/graph_bag/include/graph_bag/graph_bag_params.h index 32598d9a13..337a097337 100644 --- a/tools/graph_bag/include/graph_bag/graph_bag_params.h +++ b/tools/graph_bag/include/graph_bag/graph_bag_params.h @@ -27,6 +27,7 @@ namespace graph_bag { struct GraphBagParams { bool save_optical_flow_images; + bool log_relative_time; std::unique_ptr nav_cam_params; gtsam::Pose3 body_T_nav_cam; int ar_min_num_landmarks; diff --git a/tools/graph_bag/include/graph_bag/imu_bias_tester_adder.h b/tools/graph_bag/include/graph_bag/imu_bias_tester_adder.h new file mode 100644 index 0000000000..25b7c73ecb --- /dev/null +++ b/tools/graph_bag/include/graph_bag/imu_bias_tester_adder.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_BAG_IMU_BIAS_TESTER_ADDER_H_ +#define GRAPH_BAG_IMU_BIAS_TESTER_ADDER_H_ + +#include + +#include +#include + +#include + +namespace graph_bag { +class ImuBiasTesterAdder { + public: + ImuBiasTesterAdder(const std::string& input_bag_name, const std::string& output_bag_name); + void AddPredictions(); + + private: + imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; + rosbag::Bag input_bag_; + rosbag::Bag output_bag_; +}; +} // end namespace graph_bag + +#endif // GRAPH_BAG_IMU_BIAS_TESTER_ADDER_H_ diff --git a/tools/graph_bag/include/graph_bag/live_measurement_simulator.h b/tools/graph_bag/include/graph_bag/live_measurement_simulator.h index 3fc40a8320..ee2271b286 100644 --- a/tools/graph_bag/include/graph_bag/live_measurement_simulator.h +++ b/tools/graph_bag/include/graph_bag/live_measurement_simulator.h @@ -56,8 +56,6 @@ class LiveMeasurementSimulator { boost::optional GetFlightModeMessage(const localization_common::Time current_time); private: - bool string_ends_with(const std::string& str, const std::string& ending); - ff_msgs::Feature2dArray GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg); bool GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, ff_msgs::VisualLandmarks& vl_features); diff --git a/tools/graph_bag/include/graph_bag/utilities.h b/tools/graph_bag/include/graph_bag/utilities.h index 7360f7c1e5..a730d8e86a 100644 --- a/tools/graph_bag/include/graph_bag/utilities.h +++ b/tools/graph_bag/include/graph_bag/utilities.h @@ -20,19 +20,46 @@ #include #include +#include +#include #include +#include #include +#include +#include + namespace graph_bag { -void FeatureTrackImage(const graph_localizer::FeatureTrackMap& feature_tracks, +// TODO(rsoussan): put these somewhere else! +using Calibration = gtsam::Cal3_S2; +using Camera = gtsam::PinholePose; +using SmartFactor = gtsam::RobustSmartProjectionPoseFactor; + +void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, const camera::CameraParameters& camera_params, cv::Mat& feature_track_image); -boost::optional CreateFeatureTrackImage(const sensor_msgs::ImageConstPtr& image_msg, - const graph_localizer::FeatureTrackMap& feature_tracks, - const camera::CameraParameters& camera_params); +void MarkSmartFactorPoints(const std::vector smart_factors, + const camera::CameraParameters& camera_params, const double feature_track_min_separation, + const int max_num_factors, cv::Mat& feature_track_image); +boost::optional CreateFeatureTrackImage( + const sensor_msgs::ImageConstPtr& image_msg, const graph_localizer::FeatureTrackIdMap& feature_tracks, + const camera::CameraParameters& camera_params, const std::vector& smart_factors = {}); cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); + +std::vector SmartFactors(const graph_localizer::GraphLocalizer& graph); + +bool string_ends_with(const std::string& str, const std::string& ending); + +void SaveImuBiasTesterPredictedStates( + const std::vector& imu_bias_tester_predicted_states, rosbag::Bag& bag); + +template +void SaveMsg(const MsgType& msg, const std::string& topic, rosbag::Bag& bag) { + const ros::Time timestamp = localization_common::RosTimeFromHeader(msg.header); + bag.write("/" + topic, timestamp, msg); +} } // namespace graph_bag #endif // GRAPH_BAG_UTILITIES_H_ diff --git a/tools/graph_bag/scripts/bag_and_parameter_sweep.py b/tools/graph_bag/scripts/bag_and_parameter_sweep.py index 77730c8e30..d38b64bcab 100755 --- a/tools/graph_bag/scripts/bag_and_parameter_sweep.py +++ b/tools/graph_bag/scripts/bag_and_parameter_sweep.py @@ -75,7 +75,8 @@ def bag_and_parameter_sweep(graph_bag_params_list, output_dir): param_range_directory_for_bag = parameter_sweep.make_values_and_parameter_sweep( bag_output_dir, graph_bag_params.bagfile, graph_bag_params.map_file, graph_bag_params.image_topic, graph_bag_params.config_path, graph_bag_params.robot_config_file, graph_bag_params.world, - graph_bag_params.use_image_features) + graph_bag_params.use_image_features, graph_bag_params.groundtruth_bagfile, graph_bag_params.rmse_rel_start_time, + graph_bag_params.rmse_rel_end_time) if not param_range_directory: param_range_directory = param_range_directory_for_bag combined_results_csv_files.append(os.path.join(bag_output_dir, 'param_sweep_combined_results.csv')) diff --git a/tools/graph_bag/scripts/bag_sweep.py b/tools/graph_bag/scripts/bag_sweep.py index 9deff9a663..1350c1d392 100644 --- a/tools/graph_bag/scripts/bag_sweep.py +++ b/tools/graph_bag/scripts/bag_sweep.py @@ -30,7 +30,8 @@ class GraphBagParams(object): - def __init__(self, bagfile, map_file, image_topic, config_path, robot_config_file, world, use_image_features): + def __init__(self, bagfile, map_file, image_topic, config_path, robot_config_file, world, use_image_features, + groundtruth_bagfile, rmse_rel_start_time, rmse_rel_end_time): self.bagfile = bagfile self.map_file = map_file self.image_topic = image_topic @@ -38,6 +39,9 @@ def __init__(self, bagfile, map_file, image_topic, config_path, robot_config_fil self.robot_config_file = robot_config_file self.world = world self.use_image_features = use_image_features + self.groundtruth_bagfile = groundtruth_bagfile + self.rmse_rel_start_time = rmse_rel_start_time + self.rmse_rel_end_time = rmse_rel_end_time def load_params(param_file): @@ -45,7 +49,8 @@ def load_params(param_file): with open(param_file) as param_csvfile: reader = csv.reader(param_csvfile, delimiter=' ') for row in reader: - graph_bag_params_list.append(GraphBagParams(row[0], row[1], row[2], row[3], row[4], row[5], row[6])) + graph_bag_params_list.append( + GraphBagParams(row[0], row[1], row[2], row[3], row[4], row[5], row[6], row[7], row[8], row[9])) return graph_bag_params_list @@ -73,6 +78,9 @@ def check_params(graph_bag_params_list): if not os.path.isfile(params.map_file): print('Map file ' + params.map_file + ' does not exist.') sys.exit() + if not os.path.isfile(params.groundtruth_bagfile): + print('Bagfile ' + params.groundtruth_bagfile + ' does not exist.') + sys.exit() # Add traceback so errors are forwarded, otherwise @@ -86,7 +94,7 @@ def run_graph_bag(params, output_dir): run_command = 'rosrun graph_bag run_graph_bag ' + params.bagfile + ' ' + params.map_file + ' ' + params.config_path + ' -i ' + params.image_topic + ' -o ' + output_bag_path + ' -r ' + params.robot_config_file + ' -w ' + params.world + ' -s ' + output_csv_file + ' -f ' + params.use_image_features os.system(run_command) output_pdf_file = os.path.join(output_dir, bag_name + '_output.pdf') - plot_command = 'rosrun graph_bag plot_results_main.py ' + output_bag_path + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + plot_command = 'rosrun graph_bag plot_results_main.py ' + output_bag_path + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + ' -g ' + params.groundtruth_bagfile + ' --rmse-rel-start-time ' + params.rmse_rel_start_time + ' --rmse-rel-end-time ' + params.rmse_rel_end_time os.system(plot_command) @@ -100,7 +108,7 @@ def run_graph_bag_helper(zipped_vals): def bag_sweep(config_file, output_dir): graph_bag_params_list = load_params(config_file) check_params(graph_bag_params_list) - num_processes = 6 + num_processes = 15 pool = multiprocessing.Pool(num_processes) # izip arguments so we can pass as one argument to pool worker pool.map(run_graph_bag_helper, itertools.izip(graph_bag_params_list, itertools.repeat(output_dir))) diff --git a/tools/graph_bag/scripts/get_average_opt_and_update_times.py b/tools/graph_bag/scripts/get_average_opt_and_update_times.py new file mode 100755 index 0000000000..78dc200a33 --- /dev/null +++ b/tools/graph_bag/scripts/get_average_opt_and_update_times.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +import argparse +import os +import sys + +import numpy as np + +import rosbag + + +def get_average_opt_and_update_times(bagfile): + with rosbag.Bag(bagfile, 'r') as bag: + optimization_times = [] + update_times = [] + for _, msg, _ in bag.read_messages(['/graph_loc/state']): + optimization_times.append(msg.optimization_time) + update_times.append(msg.update_time) + + mean_optimization_time = np.mean(optimization_times) + min_optimization_time = np.min(optimization_times) + max_optimization_time = np.max(optimization_times) + stddev_optimization_time = np.std(optimization_times) + print('Mean optimization time: ' + str(mean_optimization_time)) + print('Min optimization time: ' + str(min_optimization_time)) + print('Max optimization time: ' + str(max_optimization_time)) + print('Stddev optimization time: ' + str(stddev_optimization_time)) + + mean_update_time = np.mean(update_times) + min_update_time = np.min(update_times) + max_update_time = np.max(update_times) + stddev_update_time = np.std(update_times) + print('Mean update time: ' + str(mean_update_time)) + print('Min update time: ' + str(min_update_time)) + print('Max update time: ' + str(max_update_time)) + print('Stddev update time: ' + str(stddev_update_time)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('bagfile') + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print('Bag file ' + args.bagfile + ' does not exist.') + sys.exit() + + get_average_opt_and_update_times(args.bagfile) diff --git a/tools/graph_bag/scripts/merge_all_bags.py b/tools/graph_bag/scripts/merge_all_bags.py index c37e570c67..12072090e9 100755 --- a/tools/graph_bag/scripts/merge_all_bags.py +++ b/tools/graph_bag/scripts/merge_all_bags.py @@ -29,6 +29,7 @@ if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--merged-bag', default='') + parser.add_argument('--only-loc-topics', dest='only_loc_topics', action='store_true') args = parser.parse_args() # Find bagfiles with bag prefix in current directory, fail if none found @@ -44,4 +45,4 @@ print('Found ' + str(len(bag_names)) + ' bag file prefixes.') for bag_name in bag_names: - merge_bags.merge_bag(bag_name, args.merged_bag) + merge_bags.merge_bag(bag_name, args.merged_bag, args.only_loc_topics) diff --git a/tools/graph_bag/scripts/merge_bags.py b/tools/graph_bag/scripts/merge_bags.py index 09b3f9cb9d..0abe793a28 100755 --- a/tools/graph_bag/scripts/merge_bags.py +++ b/tools/graph_bag/scripts/merge_bags.py @@ -31,7 +31,7 @@ def natural_sort(l): return sorted(l, key=alphanum_key) -def merge_bag(input_bag_prefix, merged_bag): +def merge_bag(input_bag_prefix, merged_bag, only_loc_topics=False): # Find bagfiles with bag prefix in current directory, fail if none found bag_names = [ bag for bag in os.listdir('.') if os.path.isfile(bag) and bag.startswith(input_bag_prefix) and bag.endswith('.bag') @@ -48,10 +48,13 @@ def merge_bag(input_bag_prefix, merged_bag): sorted_bag_names = natural_sort(bag_names) - topics = [ - '/hw/imu', '/loc/of/features', '/loc/ml/features', '/loc/ar/features', '/mgt/img_sampler/nav_cam/image_record', - '/graph_loc/state', '/gnc/ekf', '/sparse_mapping/pose' - ] + topics = None + if only_loc_topics: + topics = [ + '/hw/imu', '/loc/of/features', '/loc/ml/features', '/loc/ar/features', '/mgt/img_sampler/nav_cam/image_record', + '/graph_loc/state', '/gnc/ekf', '/sparse_mapping/pose', '/mob/flight_mode', '/beh/inspection/feedback', + '/beh/inspection/goal', '/beh/inspection/result' + ] with rosbag.Bag(merged_bag_name, 'w') as merged_bag: for sorted_bag_name in sorted_bag_names: @@ -64,5 +67,7 @@ def merge_bag(input_bag_prefix, merged_bag): parser = argparse.ArgumentParser() parser.add_argument('input_bag_prefix') parser.add_argument('--merged-bag', default='') + parser.add_argument('--only-loc-topics', dest='only_loc_topics', action='store_true') + parser.add_argument('--merged-bag', default='') args = parser.parse_args() - merge_bag(args.input_bag_prefix, args.merged_bag) + merge_bag(args.input_bag_prefix, args.merged_bag, args.only_loc_topics) diff --git a/tools/graph_bag/scripts/parameter_sweep.py b/tools/graph_bag/scripts/parameter_sweep.py index b987575144..54b6620b56 100755 --- a/tools/graph_bag/scripts/parameter_sweep.py +++ b/tools/graph_bag/scripts/parameter_sweep.py @@ -37,7 +37,7 @@ # library call @utilities.full_traceback def test_values(values, job_id, value_names, output_dir, bag_file, map_file, image_topic, config_path, robot_config, - world, use_image_features): + world, use_image_features, groundtruth_bagfile, rmse_rel_start_time, rmse_rel_end_time): new_output_dir = os.path.join(output_dir, str(job_id)) os.mkdir(new_output_dir) graph_config_filepath = os.path.join(config_path, "config", "graph_localizer.config") @@ -46,7 +46,6 @@ def test_values(values, job_id, value_names, output_dir, bag_file, map_file, ima output_bag = os.path.join(new_output_dir, "results.bag") output_stats_file = os.path.join(new_output_dir, "graph_stats.csv") graph_config_prefix = new_output_dir + '/' - run_command = 'rosrun graph_bag run_graph_bag ' + bag_file + ' ' + map_file + ' ' + config_path + ' -o ' + output_bag + ' -s ' + output_stats_file + ' -r ' + robot_config + ' -w ' + world + ' -g ' + graph_config_prefix + ' -f ' + str( use_image_features) if image_topic is not None: @@ -54,7 +53,8 @@ def test_values(values, job_id, value_names, output_dir, bag_file, map_file, ima os.system(run_command) output_pdf_file = os.path.join(new_output_dir, str(job_id) + '_output.pdf') output_csv_file = os.path.join(new_output_dir, 'graph_stats.csv') - plot_command = 'rosrun graph_bag plot_results_main.py ' + output_bag + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + plot_command = 'rosrun graph_bag plot_results_main.py ' + output_bag + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + ' -g ' + groundtruth_bagfile + ' --rmse-rel-start-time ' + str( + rmse_rel_start_time) + ' --rmse-rel-end-time ' + str(rmse_rel_end_time) os.system(plot_command) @@ -75,10 +75,21 @@ def concat_results(job_ids, directory): combined_results.to_csv(combined_results_file, index=False) -def parameter_sweep(all_value_combos, value_names, output_dir, bag_file, map_file, image_topic, config_path, - robot_config, world, use_image_features): +def parameter_sweep(all_value_combos, + value_names, + output_dir, + bag_file, + map_file, + image_topic, + config_path, + robot_config, + world, + use_image_features, + groundtruth_bagfile, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): job_ids = list(range(len(all_value_combos))) - num_processes = 6 + num_processes = 15 pool = multiprocessing.Pool(num_processes) # izip arguments so we can pass as one argument to pool worker pool.map( @@ -86,7 +97,8 @@ def parameter_sweep(all_value_combos, value_names, output_dir, bag_file, map_fil itertools.izip(all_value_combos, job_ids, itertools.repeat(value_names), itertools.repeat(output_dir), itertools.repeat(bag_file), itertools.repeat(map_file), itertools.repeat(image_topic), itertools.repeat(config_path), itertools.repeat(robot_config), itertools.repeat(world), - itertools.repeat(use_image_features))) + itertools.repeat(use_image_features), itertools.repeat(groundtruth_bagfile), + itertools.repeat(rmse_rel_start_time), itertools.repeat(rmse_rel_end_time))) concat_results(job_ids, output_dir) @@ -100,12 +112,10 @@ def make_value_ranges(): steps = 10 # tune num smart factors - value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) - value_names.append('accel_bias_sigma') - value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) - value_names.append('accel_sigma') - - + #value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) + #value_names.append('accel_bias_sigma') + value_ranges.append(np.linspace(0, 500, steps, endpoint=True)) + value_names.append('smart_projection_adder_feature_track_min_separation') #q_gyro # .001 -> 2 deg @@ -124,8 +134,17 @@ def save_values(value_names, values, filename, output_dir): writer.writerows(values) -def make_values_and_parameter_sweep(output_dir, bag_file, map_file, image_topic, config_path, robot_config, world, - use_image_features): +def make_values_and_parameter_sweep(output_dir, + bag_file, + map_file, + image_topic, + config_path, + robot_config, + world, + use_image_features, + groundtruth_bagfile, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): output_dir = utilities.create_directory(output_dir) print('Output directory for results is {}'.format(output_dir)) @@ -136,7 +155,7 @@ def make_values_and_parameter_sweep(output_dir, bag_file, map_file, image_topic, save_values(value_names, all_value_combos, 'all_value_combos.csv', output_dir) parameter_sweep(all_value_combos, value_names, output_dir, bag_file, map_file, image_topic, config_path, robot_config, - world, use_image_features) + world, use_image_features, groundtruth_bagfile, rmse_rel_start_time, rmse_rel_end_time) combined_results_file = os.path.join(output_dir, 'param_sweep_combined_results.csv') value_combos_file = os.path.join(output_dir, 'all_value_combos.csv') results_pdf_file = os.path.join(output_dir, 'param_sweep_results.pdf') @@ -157,6 +176,7 @@ def make_values_and_parameter_sweep(output_dir, bag_file, map_file, image_topic, action='store_false', help='Use image features msgs from bagfile or generate features from images.') + parser.add_argument('-g', '--groundtruth-bagfile', default=None) parser.add_argument( '--directory', default=None, @@ -164,6 +184,9 @@ def make_values_and_parameter_sweep(output_dir, bag_file, map_file, image_topic, 'Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.' ) args = parser.parse_args() + # Default set groundtruth bagfile to normal bagfile + if not args.groundtruth_bagfile: + args.groundtruth_bagfile = args.bag_file make_values_and_parameter_sweep(args.directory, args.bag_file, args.map_file, args.image_topic, args.config_path, - args.robot_config, args.world, args.use_image_features) + args.robot_config, args.world, args.use_image_features, args.groundtruth_bagfile) diff --git a/tools/graph_bag/scripts/plot_all_results.py b/tools/graph_bag/scripts/plot_all_results.py new file mode 100755 index 0000000000..5b3d3015b5 --- /dev/null +++ b/tools/graph_bag/scripts/plot_all_results.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import argparse +import os + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--output-dir', default='') + args = parser.parse_args() + + # Find bagfiles with bag prefix in current directory, fail if none found + bag_names = [bag for bag in os.listdir('.') if os.path.isfile(bag) and bag.endswith('.bag')] + bag_names = sorted(set(bag_names)) + if (len(bag_names) == 0): + print('No bag files found') + sys.exit() + else: + print('Found ' + str(len(bag_names)) + ' bags.') + + for bag_name in bag_names: + output_file_name = os.path.splitext(os.path.basename(bag_name))[0] + '_output.pdf' + if args.output_dir: + output_file_name = args.output_dir + '/' + output_file_name + plot_command = 'rosrun graph_bag plot_results_main.py ' + bag_name + ' --output-file ' + output_file_name + os.system(plot_command) diff --git a/tools/graph_bag/scripts/plot_bag_sweep_results.py b/tools/graph_bag/scripts/plot_bag_sweep_results.py index 3883d21418..32f6e14e10 100755 --- a/tools/graph_bag/scripts/plot_bag_sweep_results.py +++ b/tools/graph_bag/scripts/plot_bag_sweep_results.py @@ -132,15 +132,23 @@ def create_plot(output_file, csv_file, label_1='', csv_file_2=None, label_2='', dataframe.sort_values(by=['Bag'], inplace=True) # Graph rmses rmses = dataframe['rmse'] + rel_rmses = dataframe['rel_rmse'] integrated_rmses = dataframe['integrated_rmse'] + rel_integrated_rmses = dataframe['rel_integrated_rmse'] orientation_rmses = dataframe['orientation_rmse'] + rel_orientation_rmses = dataframe['rel_orientation_rmse'] # IMU augmented rmses imu_augmented_rmses = dataframe['imu_augmented_rmse'] + rel_imu_augmented_rmses = dataframe['rel_imu_augmented_rmse'] imu_augmented_integrated_rmses = dataframe['imu_augmented_integrated_rmse'] + rel_imu_augmented_integrated_rmses = dataframe['rel_imu_augmented_integrated_rmse'] imu_augmented_orientation_rmses = dataframe['imu_augmented_orientation_rmse'] + rel_imu_augmented_orientation_rmses = dataframe['rel_imu_augmented_orientation_rmse'] # IMU bias tester rmses imu_bias_tester_rmses = dataframe['imu_bias_tester_rmse'] + rel_imu_bias_tester_rmses = dataframe['rel_imu_bias_tester_rmse'] imu_bias_tester_orientation_rmses = dataframe['imu_bias_tester_orientation_rmse'] + rel_imu_bias_tester_orientation_rmses = dataframe['rel_imu_bias_tester_orientation_rmse'] bag_names = dataframe['Bag'].tolist() max_name_length = 45 @@ -149,29 +157,45 @@ def create_plot(output_file, csv_file, label_1='', csv_file_2=None, label_2='', ] x_axis_vals = range(len(shortened_bag_names)) rmses_2 = None + rel_rmses_2 = None integrated_rmses_2 = None + rel_integrated_rmses_2 = None orientation_rmses_2 = None + rel_orientation_rmses_2 = None imu_augmented_rmses_2 = None + rel_imu_augmented_rmses_2 = None imu_augmented_integrated_rmses_2 = None + rel_imu_augmented_integrated_rmses_2 = None imu_augmented_orientation_rmses_2 = None + rel_imu_augmented_orientation_rmses_2 = None imu_bias_tester_rmses_2 = None + rel_imu_bias_tester_rmses_2 = None imu_bias_tester_orientation_rmses_2 = None + rel_imu_bias_tester_orientation_rmses_2 = None if (csv_file_2): dataframe_2 = pd.read_csv(csv_file_2) dataframe_2.sort_values(by=['Bag'], inplace=True) # Graph rmses rmses_2 = dataframe_2['rmse'] + rel_rmses_2 = dataframe_2['rel_rmse'] integrated_rmses_2 = dataframe_2['integrated_rmse'] + rel_integrated_rmses_2 = dataframe_2['rel_integrated_rmse'] orientation_rmses_2 = dataframe_2['orientation_rmse'] + rel_orientation_rmses_2 = dataframe_2['rel_orientation_rmse'] if (imu_augmented_2): # IMU augmented rmses imu_augmented_rmses_2 = dataframe_2['imu_augmented_rmse'] + rel_imu_augmented_rmses_2 = dataframe_2['rel_imu_augmented_rmse'] imu_augmented_integrated_rmses_2 = dataframe_2['imu_augmented_integrated_rmse'] + rel_imu_augmented_integrated_rmses_2 = dataframe_2['rel_imu_augmented_integrated_rmse'] imu_augmented_orientation_rmses_2 = dataframe_2['imu_augmented_orientation_rmse'] + rel_imu_augmented_orientation_rmses_2 = dataframe_2['rel_imu_augmented_orientation_rmse'] # IMU bias tester rmses imu_bias_tester_rmses_2 = dataframe_2['imu_bias_tester_rmse'] + rel_imu_bias_tester_rmses_2 = dataframe_2['rel_imu_bias_tester_rmse'] imu_bias_tester_orientation_rmses_2 = dataframe_2['imu_bias_tester_orientation_rmse'] + rel_imu_bias_tester_orientation_rmses_2 = dataframe_2['rel_imu_bias_tester_orientation_rmse'] bag_names_2 = dataframe_2['Bag'].tolist() if bag_names != bag_names_2: @@ -180,20 +204,35 @@ def create_plot(output_file, csv_file, label_1='', csv_file_2=None, label_2='', with PdfPages(output_file) as pdf: rmse_plots(pdf, x_axis_vals, shortened_bag_names, rmses, integrated_rmses, orientation_rmses, '', label_1, rmses_2, integrated_rmses_2, orientation_rmses_2, label_2) + rmse_plots(pdf, x_axis_vals, shortened_bag_names, rel_rmses, rel_integrated_rmses, rel_orientation_rmses, 'rel', + label_1, rel_rmses_2, rel_integrated_rmses_2, rel_orientation_rmses_2, label_2) if imu_augmented_2: rmse_plots(pdf, x_axis_vals, shortened_bag_names, imu_augmented_rmses, imu_augmented_integrated_rmses, imu_augmented_orientation_rmses, 'imu_augmented', label_1, imu_augmented_rmses_2, imu_augmented_integrated_rmses_2, imu_augmented_orientation_rmses_2, label_2) + rmse_plots(pdf, x_axis_vals, shortened_bag_names, rel_imu_augmented_rmses, rel_imu_augmented_integrated_rmses, + rel_imu_augmented_orientation_rmses, 'rel_imu_augmented', label_1, rel_imu_augmented_rmses_2, + rel_imu_augmented_integrated_rmses_2, rel_imu_augmented_orientation_rmses_2, label_2) rmse_plots(pdf, x_axis_vals, shortened_bag_names, imu_bias_tester_rmses, None, imu_bias_tester_orientation_rmses, 'imu_bias_tester', label_1, imu_bias_tester_rmses_2, None, imu_bias_tester_orientation_rmses_2, label_2) + rmse_plots(pdf, x_axis_vals, shortened_bag_names, rel_imu_bias_tester_rmses, None, + rel_imu_bias_tester_orientation_rmses, 'rel_imu_bias_tester', label_1, rel_imu_bias_tester_rmses_2, + None, rel_imu_bias_tester_orientation_rmses_2, label_2) + else: rmse_plots(pdf, x_axis_vals, shortened_bag_names, imu_augmented_rmses, imu_augmented_integrated_rmses, imu_augmented_orientation_rmses, 'imu_augmented', label_1, rmses_2, integrated_rmses_2, orientation_rmses_2, label_2 + ' no imu aug') + rmse_plots(pdf, x_axis_vals, shortened_bag_names, rel_imu_augmented_rmses, rel_imu_augmented_integrated_rmses, + rel_imu_augmented_orientation_rmses, 'rel_imu_augmented', label_1, rel_rmses_2, rel_integrated_rmses_2, + rel_orientation_rmses_2, label_2 + ' no imu aug') rmse_plots(pdf, x_axis_vals, shortened_bag_names, imu_bias_tester_rmses, None, imu_bias_tester_orientation_rmses, 'imu_bias_tester', label_1, imu_bias_tester_rmses_2, None, imu_bias_tester_orientation_rmses_2, label_2) + rmse_plots(pdf, x_axis_vals, shortened_bag_names, rel_imu_bias_tester_rmses, None, + rel_imu_bias_tester_orientation_rmses, 'rel_imu_bias_tester', label_1, rel_imu_bias_tester_rmses_2, + None, rel_imu_bias_tester_orientation_rmses_2, label_2) if __name__ == '__main__': diff --git a/tools/graph_bag/scripts/plot_parameter_sweep_results.py b/tools/graph_bag/scripts/plot_parameter_sweep_results.py index 7ac00d6b53..72ae0c7cab 100755 --- a/tools/graph_bag/scripts/plot_parameter_sweep_results.py +++ b/tools/graph_bag/scripts/plot_parameter_sweep_results.py @@ -63,7 +63,7 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=''): last_x_val = x_axis_vals[len(x_axis_vals) - 1] # Use log scale if min and max x vals are more than 3 orders of magnitude apart if (first_x_val != 0 and last_x_val != 0 and abs(math.log10(last_x_val) - math.log10(first_x_val)) > 3): - plt.xscale('log', base=10) + plt.xscale('log', basex=10) # Extend x axis on either side using a log scale to make data more visible if (first_x_val < last_x_val): plt.xlim([first_x_val * 0.1, last_x_val * 10.0]) @@ -86,13 +86,22 @@ def create_plots(output_file, csv_file, value_combos_file): create_plot(pdf, csv_file, value_combos_file) create_plot(pdf, csv_file, value_combos_file, 'orientation_') create_plot(pdf, csv_file, value_combos_file, 'integrated_') + create_plot(pdf, csv_file, value_combos_file, 'rel_') + create_plot(pdf, csv_file, value_combos_file, 'rel_orientation_') + create_plot(pdf, csv_file, value_combos_file, 'rel_integrated_') # IMU Augmented RMSEs create_plot(pdf, csv_file, value_combos_file, 'imu_augmented_') create_plot(pdf, csv_file, value_combos_file, 'imu_augmented_orientation_') create_plot(pdf, csv_file, value_combos_file, 'imu_augmented_integrated_') + create_plot(pdf, csv_file, value_combos_file, 'rel_imu_augmented_') + create_plot(pdf, csv_file, value_combos_file, 'rel_imu_augmented_orientation_') + create_plot(pdf, csv_file, value_combos_file, 'rel_imu_augmented_integrated_') + # IMU Bias Tester RMSEs create_plot(pdf, csv_file, value_combos_file, 'imu_bias_tester_') create_plot(pdf, csv_file, value_combos_file, 'imu_bias_tester_orientation_') + create_plot(pdf, csv_file, value_combos_file, 'rel_imu_bias_tester_') + create_plot(pdf, csv_file, value_combos_file, 'rel_imu_bias_tester_orientation_') if __name__ == '__main__': diff --git a/tools/graph_bag/scripts/plot_results.py b/tools/graph_bag/scripts/plot_results.py index 66e3f909ff..ff6fed1b39 100644 --- a/tools/graph_bag/scripts/plot_results.py +++ b/tools/graph_bag/scripts/plot_results.py @@ -38,7 +38,7 @@ def l2_map(vector3ds): - return map(lambda (x, y, z): math.sqrt(x + y + z), zip(vector3ds.xs, vector3ds.ys, vector3ds.zs)) + return map(lambda (x, y, z): math.sqrt(x*x + y*y + z*z), zip(vector3ds.xs, vector3ds.ys, vector3ds.zs)) def add_graph_plots(pdf, sparse_mapping_poses, ar_tag_poses, graph_localization_states, @@ -52,11 +52,7 @@ def add_graph_plots(pdf, sparse_mapping_poses, ar_tag_poses, graph_localization_ markeredgewidth=0.1, markersize=1.5) if ar_tag_poses.times: - position_plotter.add_pose_position(ar_tag_poses, - linestyle='None', - marker='x', - markeredgewidth=0.1, - markersize=1.5) + position_plotter.add_pose_position(ar_tag_poses, linestyle='None', marker='x', markeredgewidth=0.1, markersize=1.5) position_plotter.add_pose_position(graph_localization_states) position_plotter.plot(pdf) @@ -115,11 +111,7 @@ def add_graph_plots(pdf, sparse_mapping_poses, ar_tag_poses, graph_localization_ markeredgewidth=0.1, markersize=1.5) if ar_tag_poses.times: - position_plotter.add_pose_position(ar_tag_poses, - linestyle='None', - marker='x', - markeredgewidth=0.1, - markersize=1.5) + position_plotter.add_pose_position(ar_tag_poses, linestyle='None', marker='x', markeredgewidth=0.1, markersize=1.5) integrated_graph_localization_states = utilities.integrate_velocities(graph_localization_states) position_plotter.add_pose_position(integrated_graph_localization_states) @@ -379,13 +371,31 @@ def plot_loc_state_stats(pdf, output_csv_file, prefix='', atol=0, - plot_integrated_velocities=True): - rmse = rmse_utilities.rmse_timestamped_poses(localization_states, sparse_mapping_poses, True, atol) + plot_integrated_velocities=True, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): + plot_loc_state_stats_abs(pdf, localization_states, sparse_mapping_poses, output_csv_file, prefix, atol, + plot_integrated_velocities, rmse_rel_start_time, rmse_rel_end_time) + plot_loc_state_stats_rel(pdf, localization_states, sparse_mapping_poses, output_csv_file, prefix, atol, + plot_integrated_velocities, rmse_rel_start_time, rmse_rel_end_time) + + +def plot_loc_state_stats_abs(pdf, + localization_states, + sparse_mapping_poses, + output_csv_file, + prefix='', + atol=0, + plot_integrated_velocities=True, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): + rmse = rmse_utilities.rmse_timestamped_poses(localization_states, sparse_mapping_poses, True, atol, + rmse_rel_start_time, rmse_rel_end_time) integrated_rmse = [] if plot_integrated_velocities: integrated_localization_states = utilities.integrate_velocities(localization_states) integrated_rmse = rmse_utilities.rmse_timestamped_poses(integrated_localization_states, sparse_mapping_poses, False, - atol) + atol, rmse_rel_start_time, rmse_rel_end_time) stats = prefix + ' pos rmse: ' + str(rmse[0]) + '\n' + 'orientation rmse: ' + str(rmse[1]) if plot_integrated_velocities: stats += '\n' + 'integrated rmse: ' + str(integrated_rmse[0]) @@ -401,6 +411,39 @@ def plot_loc_state_stats(pdf, pdf.savefig() +def plot_loc_state_stats_rel(pdf, + localization_states, + sparse_mapping_poses, + output_csv_file, + prefix='', + atol=0, + plot_integrated_velocities=True, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): + rmse = rmse_utilities.rmse_timestamped_poses_relative(localization_states, sparse_mapping_poses, True, atol, + rmse_rel_start_time, rmse_rel_end_time) + integrated_rmse = [] + if plot_integrated_velocities: + integrated_localization_states = utilities.integrate_velocities(localization_states) + integrated_rmse = rmse_utilities.rmse_timestamped_poses_relative(integrated_localization_states, + sparse_mapping_poses, False, atol, + rmse_rel_start_time, rmse_rel_end_time) + stats = prefix + ' rel pos rmse: ' + str(rmse[0]) + '\n' + 'rel orientation rmse: ' + str(rmse[1]) + if plot_integrated_velocities: + stats += '\n' + 'rel integrated rmse: ' + str(integrated_rmse[0]) + + with open(output_csv_file, 'a') as output_csv: + csv_writer = csv.writer(output_csv, lineterminator='\n') + csv_writer.writerow(['rel_' + prefix + 'rmse', str(rmse[0])]) + csv_writer.writerow(['rel_' + prefix + 'orientation_rmse', str(rmse[1])]) + if plot_integrated_velocities: + csv_writer.writerow(['rel_' + prefix + 'integrated_rmse', str(integrated_rmse[0])]) + plt.figure() + plt.axis('off') + plt.text(0.0, 0.5, stats) + pdf.savefig() + + def add_imu_bias_tester_poses(pdf, imu_bias_tester_poses, sparse_mapping_poses): colors = ['r', 'b', 'g'] plt.figure() @@ -481,8 +524,15 @@ def has_topic(bag, topic): return topic in topics -def create_plots(bagfile, output_pdf_file, output_csv_file='results.csv'): +# Groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed +def create_plots(bagfile, + output_pdf_file, + output_csv_file='results.csv', + groundtruth_bagfile=None, + rmse_rel_start_time=0, + rmse_rel_end_time=-1): bag = rosbag.Bag(bagfile) + groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag bag_start_time = bag.get_start_time() has_imu_augmented_graph_localization_state = has_topic(bag, '/gnc/ekf') @@ -490,8 +540,10 @@ def create_plots(bagfile, output_pdf_file, output_csv_file='results.csv'): sparse_mapping_poses = poses.Poses('Sparse Mapping', '/sparse_mapping/pose') ar_tag_poses = poses.Poses('AR Tag', '/ar_tag/pose') imu_bias_tester_poses = poses.Poses('Imu Bias Tester', '/imu_bias_tester/pose') - vec_of_poses = [sparse_mapping_poses, ar_tag_poses, imu_bias_tester_poses] + vec_of_poses = [ar_tag_poses, imu_bias_tester_poses] load_pose_msgs(vec_of_poses, bag, bag_start_time) + groundtruth_vec_of_poses = [sparse_mapping_poses] + load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time) graph_localization_states = loc_states.LocStates('Graph Localization', '/graph_loc/state') imu_augmented_graph_localization_states = loc_states.LocStates('Imu Augmented Graph Localization', '/gnc/ekf') @@ -514,9 +566,27 @@ def create_plots(bagfile, output_pdf_file, output_csv_file='results.csv'): ar_tag_poses) else: add_other_loc_plots(pdf, graph_localization_states, graph_localization_states) - plot_loc_state_stats(pdf, graph_localization_states, sparse_mapping_poses, output_csv_file) - plot_loc_state_stats(pdf, imu_augmented_graph_localization_states, sparse_mapping_poses, output_csv_file, - 'imu_augmented_', 0.01) + plot_loc_state_stats(pdf, + graph_localization_states, + sparse_mapping_poses, + output_csv_file, + rmse_rel_start_time=rmse_rel_start_time, + rmse_rel_end_time=rmse_rel_end_time) + plot_loc_state_stats(pdf, + imu_augmented_graph_localization_states, + sparse_mapping_poses, + output_csv_file, + 'imu_augmented_', + 0.01, + rmse_rel_start_time=rmse_rel_start_time, + rmse_rel_end_time=rmse_rel_end_time) if has_imu_bias_tester_poses: - plot_loc_state_stats(pdf, imu_bias_tester_poses, sparse_mapping_poses, output_csv_file, 'imu_bias_tester_', 0.01, - False) + plot_loc_state_stats(pdf, + imu_bias_tester_poses, + sparse_mapping_poses, + output_csv_file, + 'imu_bias_tester_', + 0.01, + False, + rmse_rel_start_time=rmse_rel_start_time, + rmse_rel_end_time=rmse_rel_end_time) diff --git a/tools/graph_bag/scripts/plot_results_main.py b/tools/graph_bag/scripts/plot_results_main.py index b9478899ab..a52f8e409b 100755 --- a/tools/graph_bag/scripts/plot_results_main.py +++ b/tools/graph_bag/scripts/plot_results_main.py @@ -29,8 +29,15 @@ parser.add_argument('bagfile') parser.add_argument('--output-file', default='output.pdf') parser.add_argument('--output-csv-file', default='results.csv') + parser.add_argument('-g', '--groundtruth-bagfile', default=None) + parser.add_argument('--rmse-rel-start-time', type=float, default=0) + parser.add_argument('--rmse-rel-end-time', type=float, default=-1) args = parser.parse_args() if not os.path.isfile(args.bagfile): print('Bag file ' + args.bagfile + ' does not exist.') sys.exit() - plot_results.create_plots(args.bagfile, args.output_file, args.output_csv_file) + if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): + print('Groundtruth Bag file ' + args.groundtruth_bagfile + ' does not exist.') + sys.exit() + plot_results.create_plots(args.bagfile, args.output_file, args.output_csv_file, args.groundtruth_bagfile, + args.rmse_rel_start_time, args.rmse_rel_end_time) diff --git a/tools/graph_bag/scripts/rmse_utilities.py b/tools/graph_bag/scripts/rmse_utilities.py index a43e2ab9d4..cf77837ff8 100644 --- a/tools/graph_bag/scripts/rmse_utilities.py +++ b/tools/graph_bag/scripts/rmse_utilities.py @@ -22,11 +22,12 @@ import numpy as np import scipy.spatial.transform +import bisect import math # Assumes poses_a and poses_b are sorted in time -def get_same_timestamp_poses(poses_a, poses_b, add_orientations=True, abs_tol=0): +def get_same_timestamp_poses(poses_a, poses_b, add_orientations=True, abs_tol=0, rel_start_time=0, rel_end_time=-1): trimmed_poses_a = poses.Poses(poses_a.pose_type, poses_a.topic) trimmed_poses_b = poses.Poses(poses_b.pose_type, poses_b.topic) poses_a_size = len(poses_a.times) @@ -38,6 +39,18 @@ def get_same_timestamp_poses(poses_a, poses_b, add_orientations=True, abs_tol=0) a_time = poses_a.times[a_index] b_time = poses_b.times[b_index] + # Check if times are within given start and end time bounds + if a_time < rel_start_time: + a_index += 1 + continue + if b_time < rel_start_time: + b_index += 1 + continue + # rel_end_time less than zero indicates no bound on end time + if rel_end_time >= 0: + if a_time > rel_end_time or b_time > rel_end_time: + break + if (np.isclose(a_time, b_time, rtol=0, atol=abs_tol)): trimmed_poses_a.positions.add_vector3d(poses_a.positions.get_vector3d(a_index)) trimmed_poses_a.times.append(poses_a.times[a_index]) @@ -67,8 +80,9 @@ def orientation_squared_difference(world_R_a, world_R_b): # RMSE between two sequences of poses. Only uses poses with the same timestamp -def rmse_timestamped_poses(poses_a, poses_b, add_orientation_rmse=True, abs_tol=0): - trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses(poses_a, poses_b, add_orientation_rmse, abs_tol) +def rmse_timestamped_poses(poses_a, poses_b, add_orientation_rmse=True, abs_tol=0, rel_start_time=0, rel_end_time=-1): + trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses(poses_a, poses_b, add_orientation_rmse, abs_tol, + rel_start_time, rel_end_time) assert len(trimmed_poses_a.times) == len(trimmed_poses_b.times), 'Length mismatch of poses' num_poses = len(trimmed_poses_a.times) mean_squared_position_error = 0 @@ -89,3 +103,50 @@ def rmse_timestamped_poses(poses_a, poses_b, add_orientation_rmse=True, abs_tol= position_rmse = math.sqrt(mean_squared_position_error) orientation_rmse = math.sqrt(mean_squared_orientation_error) return position_rmse, orientation_rmse + + +# Relative RMSE between two sequences of poses. Only uses poses with the same timestamp +def rmse_timestamped_poses_relative(poses_a, + poses_b, + add_orientation_rmse=True, + abs_tol=0, + rel_start_time=0, + rel_end_time=-1, + min_relative_elapsed_time=10, max_relative_elapsed_time=20): + trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses(poses_a, poses_b, add_orientation_rmse, abs_tol, + rel_start_time, rel_end_time) + assert len(trimmed_poses_a.times) == len(trimmed_poses_b.times), 'Length mismatch of poses' + num_poses = len(trimmed_poses_a.times) + mean_squared_position_error = 0 + mean_squared_orientation_error = 0 + count = 0 + for index1 in range(num_poses): + # Position Error + a_vec1 = trimmed_poses_a.positions.get_numpy_vector(index1) + b_vec1 = trimmed_poses_b.positions.get_numpy_vector(index1) + time1 = trimmed_poses_a.times[index1] + index2 = bisect.bisect_left(trimmed_poses_a.times, time1 + min_relative_elapsed_time) + if (index2 == len(trimmed_poses_a.times)): + continue + time2 = trimmed_poses_a.times[index2] + if (time2 - time1 > max_relative_elapsed_time): + continue + a_vec2 = trimmed_poses_a.positions.get_numpy_vector(index2) + b_vec2 = trimmed_poses_b.positions.get_numpy_vector(index2) + a_rel_vec = a_vec2 - a_vec1 + b_rel_vec = b_vec2 - b_vec1 + + position_squared_error = position_squared_difference(a_rel_vec, b_rel_vec) + count += 1 + # Use rolling mean to avoid overflow + mean_squared_position_error += (position_squared_error - mean_squared_position_error) / float(count) + # Orientation Error + if add_orientation_rmse: + # TODO(rsoussan): Add relative calculations for orientations + a_rot = trimmed_poses_a.orientations.get_rotation(index1) + b_rot = trimmed_poses_b.orientations.get_rotation(index1) + orientation_squared_error = orientation_squared_difference(a_rot, b_rot) + mean_squared_orientation_error += (orientation_squared_error - mean_squared_orientation_error) / float(count) + position_rmse = math.sqrt(mean_squared_position_error) + orientation_rmse = math.sqrt(mean_squared_orientation_error) + return position_rmse, orientation_rmse diff --git a/tools/graph_bag/scripts/trim_bag.py b/tools/graph_bag/scripts/trim_bag.py new file mode 100755 index 0000000000..9321090e68 --- /dev/null +++ b/tools/graph_bag/scripts/trim_bag.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +import utilities + +import argparse +import os +import sys + +import rosbag + + +def trim_bag(bag_name, start_time_to_trim, end_time_to_trim): + with rosbag.Bag(bag_name, 'r') as bag: + start_time = bag.get_start_time() + new_start_time = start_time + start_time_to_trim + end_time = bag.get_end_time() + new_end_time = end_time - end_time_to_trim + output_bag_name = os.path.splitext(bag_name)[0] + '_trimmed.bag' + run_command = 'rosbag filter ' + bag_name + ' ' + output_bag_name + ' \"t.secs >= ' + str( + new_start_time) + ' and t.secs <= ' + str(new_end_time) + '\"' + os.system(run_command) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('bagfile') + parser.add_argument('-s', '--start-time-to-trim', type=float, default=0) + parser.add_argument('-e', '--end-time-to-trim', type=float, default=0) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print('Bag file ' + args.bagfile + ' does not exist.') + sys.exit() + + trim_bag(args.bagfile, args.start_time_to_trim, args.end_time_to_trim) diff --git a/tools/graph_bag/scripts/utilities.py b/tools/graph_bag/scripts/utilities.py index d8eb986ab0..c6032264c7 100644 --- a/tools/graph_bag/scripts/utilities.py +++ b/tools/graph_bag/scripts/utilities.py @@ -133,5 +133,15 @@ def integrate_velocities(localization_states): localization_states.positions.zs[0] + cumulative_z_increment for cumulative_z_increment in cumulative_z_increments ] + # Add start positions + integrated_positions.positions.xs.insert(0, localization_states.positions.xs[0]) + integrated_positions.positions.ys.insert(0, localization_states.positions.ys[0]) + integrated_positions.positions.zs.insert(0, localization_states.positions.zs[0]) + + # Remove last elements (no timestamp for these) + del integrated_positions.positions.xs[-1] + del integrated_positions.positions.ys[-1] + del integrated_positions.positions.zs[-1] + integrated_positions.times = localization_states.times return integrated_positions diff --git a/tools/graph_bag/src/bag_imu_filterer.cc b/tools/graph_bag/src/bag_imu_filterer.cc index 39f372aa74..98d53a6234 100644 --- a/tools/graph_bag/src/bag_imu_filterer.cc +++ b/tools/graph_bag/src/bag_imu_filterer.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -25,17 +26,6 @@ #include -namespace { -// TODO(rsoussan): Unify this with live measurement simulator, put in utilities -bool string_ends_with(const std::string& str, const std::string& ending) { - if (str.length() >= ending.length()) { - return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); - } else { - return false; - } -} -} // namespace - namespace graph_bag { namespace ii = imu_integration; namespace lc = localization_common; diff --git a/tools/graph_bag/src/graph_bag.cc b/tools/graph_bag/src/graph_bag.cc index d9eeb45544..d030b58781 100644 --- a/tools/graph_bag/src/graph_bag.cc +++ b/tools/graph_bag/src/graph_bag.cc @@ -75,24 +75,13 @@ GraphBag::GraphBag(const std::string& bag_name, const std::string& map_file, con } void GraphBag::SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, - const graph_localizer::FeatureTrackMap& feature_tracks) { - const auto feature_track_image_msg = CreateFeatureTrackImage(image_msg, feature_tracks, *params_.nav_cam_params); + const GraphLocalizerSimulator& graph_localizer) { + std::vector smart_factors; + if (graph_localizer.graph_localizer()) smart_factors = SmartFactors(*(graph_localizer.graph_localizer())); + const auto feature_track_image_msg = + CreateFeatureTrackImage(image_msg, *(graph_localizer.feature_tracks()), *params_.nav_cam_params, smart_factors); if (!feature_track_image_msg) return; - SaveMsg(*image_msg, kFeatureTracksImageTopic_); -} - -void GraphBag::SaveImuBiasTesterPredictedStates( - const std::vector& imu_bias_tester_predicted_states) { - for (const auto& state : imu_bias_tester_predicted_states) { - geometry_msgs::PoseStamped pose_msg; - lc::PoseToMsg(state.pose(), pose_msg.pose); - lc::TimeToHeader(state.timestamp(), pose_msg.header); - SaveMsg(pose_msg, TOPIC_IMU_BIAS_TESTER_POSE); - geometry_msgs::Vector3Stamped velocity_msg; - mc::VectorToMsg(state.velocity(), velocity_msg.vector); - lc::TimeToHeader(state.timestamp(), velocity_msg.header); - SaveMsg(velocity_msg, TOPIC_IMU_BIAS_TESTER_VELOCITY); - } + SaveMsg(**feature_track_image_msg, kFeatureTracksImageTopic_, results_bag_); } void GraphBag::Run() { @@ -100,8 +89,10 @@ void GraphBag::Run() { graph_localizer_simulator_->ResetBiasesAndLocalizer(); lc::Timer graph_bag_timer("Graph Bag Timer"); graph_bag_timer.Start(); + const double start_time = live_measurement_simulator_->CurrentTime(); while (live_measurement_simulator_->ProcessMessage()) { const lc::Time current_time = live_measurement_simulator_->CurrentTime(); + if (params_.log_relative_time) LogInfo("Run: Rel t: " << current_time - start_time); const auto flight_mode_msg = live_measurement_simulator_->GetFlightModeMessage(current_time); if (flight_mode_msg) { graph_localizer_simulator_->BufferFlightModeMsg(*flight_mode_msg); @@ -118,7 +109,7 @@ void GraphBag::Run() { if (!imu_augmented_loc_msg) { LogWarningEveryN(50, "Run: Failed to get latest imu augmented loc msg."); } else { - SaveMsg(*imu_augmented_loc_msg, TOPIC_GNC_EKF); + SaveMsg(*imu_augmented_loc_msg, TOPIC_GNC_EKF, results_bag_); } } const auto of_msg = live_measurement_simulator_->GetOFMessage(current_time); @@ -127,7 +118,7 @@ void GraphBag::Run() { if (params_.save_optical_flow_images) { const auto img_msg = live_measurement_simulator_->GetImageMessage(lc::TimeFromHeader(of_msg->header)); if (img_msg && graph_localizer_simulator_->feature_tracks()) - SaveOpticalFlowTracksImage(*img_msg, *(graph_localizer_simulator_->feature_tracks())); + SaveOpticalFlowTracksImage(*img_msg, *graph_localizer_simulator_); } } const auto vl_msg = live_measurement_simulator_->GetVLMessage(current_time); @@ -136,7 +127,8 @@ void GraphBag::Run() { if (gl::ValidVLMsg(*vl_msg, params_.sparse_mapping_min_num_landmarks)) { const gtsam::Pose3 sparse_mapping_global_T_body = lc::GtPose(*vl_msg, params_.body_T_nav_cam.inverse()); const lc::Time timestamp = lc::TimeFromHeader(vl_msg->header); - SaveMsg(graph_localizer::PoseMsg(sparse_mapping_global_T_body, timestamp), TOPIC_SPARSE_MAPPING_POSE); + SaveMsg(graph_localizer::PoseMsg(sparse_mapping_global_T_body, timestamp), TOPIC_SPARSE_MAPPING_POSE, + results_bag_); } } const auto ar_msg = live_measurement_simulator_->GetARMessage(current_time); @@ -160,7 +152,7 @@ void GraphBag::Run() { // wrapper in the graph localizer simulator and LatestARTagPoseMsg returns // the last pose that has already been added to the graph localizer wrapper. if (last_added_timestamp != timestamp) { - SaveMsg(*ar_tag_pose_msg, TOPIC_AR_TAG_POSE); + SaveMsg(*ar_tag_pose_msg, TOPIC_AR_TAG_POSE, results_bag_); last_added_timestamp = timestamp; } } @@ -176,16 +168,16 @@ void GraphBag::Run() { LogWarningEveryN(50, "Run: Failed to get localization msg."); } else { imu_augmentor_wrapper_.LocalizationStateCallback(*localization_msg); - SaveMsg(*localization_msg, TOPIC_GRAPH_LOC_STATE); + SaveMsg(*localization_msg, TOPIC_GRAPH_LOC_STATE, results_bag_); const auto imu_bias_tester_predicted_states = imu_bias_tester_wrapper_.LocalizationStateCallback(*localization_msg); - SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states); + SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states, results_bag_); } } } graph_bag_timer.Stop(); graph_bag_timer.Log(); - const auto graph_stats = graph_localizer_simulator_->graph_stats(); + const auto graph_stats = graph_localizer_simulator_->graph_localizer_stats(); if (!graph_stats) { LogError("Run: Failed to get graph stats"); } else { diff --git a/tools/graph_bag/src/imu_bias_tester_adder.cc b/tools/graph_bag/src/imu_bias_tester_adder.cc new file mode 100644 index 0000000000..206c1dad6c --- /dev/null +++ b/tools/graph_bag/src/imu_bias_tester_adder.cc @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +namespace graph_bag { +ImuBiasTesterAdder::ImuBiasTesterAdder(const std::string& input_bag_name, const std::string& output_bag_name) + : input_bag_(input_bag_name, rosbag::bagmode::Read), output_bag_(output_bag_name, rosbag::bagmode::Write) {} + +void ImuBiasTesterAdder::AddPredictions() { + rosbag::View view(input_bag_); + for (const rosbag::MessageInstance msg : view) { + if (string_ends_with(msg.getTopic(), TOPIC_GRAPH_LOC_STATE)) { + const ff_msgs::GraphState::ConstPtr localization_msg = msg.instantiate(); + const auto imu_bias_tester_predicted_states = + imu_bias_tester_wrapper_.LocalizationStateCallback(*localization_msg); + SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states, output_bag_); + } else if (string_ends_with(msg.getTopic(), TOPIC_HARDWARE_IMU)) { + sensor_msgs::ImuConstPtr imu_msg = msg.instantiate(); + imu_bias_tester_wrapper_.ImuCallback(*imu_msg); + } + output_bag_.write(msg.getTopic(), msg.getTime(), msg); + } +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/live_measurement_simulator.cc b/tools/graph_bag/src/live_measurement_simulator.cc index 28c08bb2b8..fc00a7f18a 100644 --- a/tools/graph_bag/src/live_measurement_simulator.cc +++ b/tools/graph_bag/src/live_measurement_simulator.cc @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -52,14 +53,13 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato std::vector topics; topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); topics.push_back(TOPIC_HARDWARE_IMU); + topics.push_back(std::string("/") + kImageTopic_); + topics.push_back(kImageTopic_); if (params_.use_image_features) { topics.push_back(std::string("/") + TOPIC_LOCALIZATION_OF_FEATURES); topics.push_back(TOPIC_LOCALIZATION_OF_FEATURES); topics.push_back(std::string("/") + TOPIC_LOCALIZATION_ML_FEATURES); topics.push_back(TOPIC_LOCALIZATION_ML_FEATURES); - } else { - topics.push_back(std::string("/") + kImageTopic_); - topics.push_back(kImageTopic_); } // Only use recorded ar features topics.push_back(std::string("/") + TOPIC_LOCALIZATION_AR_FEATURES); @@ -72,14 +72,6 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato current_time_ = lc::TimeFromRosTime(view_->getBeginTime()); } -bool LiveMeasurementSimulator::string_ends_with(const std::string& str, const std::string& ending) { - if (str.length() >= ending.length()) { - return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); - } else { - return false; - } -} - ff_msgs::Feature2dArray LiveMeasurementSimulator::GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg) { ff_msgs::Feature2dArray of_features; optical_flow_tracker_.OpticalFlow(image_msg, &of_features); @@ -109,7 +101,6 @@ bool LiveMeasurementSimulator::ProcessMessage() { if (*view_it_ == view_->end()) return false; const auto& msg = **view_it_; current_time_ = lc::TimeFromRosTime(msg.getTime()); - if (string_ends_with(msg.getTopic(), TOPIC_HARDWARE_IMU)) { sensor_msgs::ImuConstPtr imu_msg = msg.instantiate(); imu_buffer_.BufferMessage(*imu_msg); @@ -120,14 +111,12 @@ bool LiveMeasurementSimulator::ProcessMessage() { // Always use ar features until have data with dock cam images const ff_msgs::VisualLandmarksConstPtr ar_features = msg.instantiate(); ar_buffer_.BufferMessage(*ar_features); - } else if (params_.use_image_features) { - if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { - const ff_msgs::Feature2dArrayConstPtr of_features = msg.instantiate(); - of_buffer_.BufferMessage(*of_features); - } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { - const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate(); - vl_buffer_.BufferMessage(*vl_features); - } + } else if (params_.use_image_features && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { + const ff_msgs::Feature2dArrayConstPtr of_features = msg.instantiate(); + of_buffer_.BufferMessage(*of_features); + } else if (params_.use_image_features && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { + const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate(); + vl_buffer_.BufferMessage(*vl_features); } else if (string_ends_with(msg.getTopic(), kImageTopic_)) { sensor_msgs::ImageConstPtr image_msg = msg.instantiate(); if (params_.save_optical_flow_images) { @@ -142,8 +131,6 @@ bool LiveMeasurementSimulator::ProcessMessage() { vl_buffer_.BufferMessage(vl_features); } } - } else { - return true; } return true; } diff --git a/tools/graph_bag/src/parameter_reader.cc b/tools/graph_bag/src/parameter_reader.cc index 8bfee1bb3d..d487e34773 100644 --- a/tools/graph_bag/src/parameter_reader.cc +++ b/tools/graph_bag/src/parameter_reader.cc @@ -50,6 +50,7 @@ void LoadGraphLocalizerSimulatorParams(config_reader::ConfigReader& config, Grap void LoadGraphBagParams(config_reader::ConfigReader& config, GraphBagParams& params) { params.save_optical_flow_images = mc::LoadBool(config, "save_optical_flow_images"); + params.log_relative_time = mc::LoadBool(config, "log_relative_time"); params.nav_cam_params.reset(new camera::CameraParameters(&config, "nav_cam")); params.body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); params.sparse_mapping_min_num_landmarks = mc::LoadInt(config, "loc_adder_min_num_matches"); diff --git a/tools/graph_bag/src/sparse_mapping_pose_adder.cc b/tools/graph_bag/src/sparse_mapping_pose_adder.cc index 94dc4aadd6..65a03d900d 100644 --- a/tools/graph_bag/src/sparse_mapping_pose_adder.cc +++ b/tools/graph_bag/src/sparse_mapping_pose_adder.cc @@ -18,22 +18,12 @@ #include #include +#include #include #include #include -namespace { -// TODO(rsoussan): Unify this with live measurement simulator, put in utilities -bool string_ends_with(const std::string& str, const std::string& ending) { - if (str.length() >= ending.length()) { - return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); - } else { - return false; - } -} -} // namespace - namespace graph_bag { namespace lc = localization_common; namespace lm = localization_measurements; diff --git a/tools/graph_bag/src/utilities.cc b/tools/graph_bag/src/utilities.cc index be50a857a7..0c6fe27ad9 100644 --- a/tools/graph_bag/src/utilities.cc +++ b/tools/graph_bag/src/utilities.cc @@ -16,10 +16,12 @@ * under the License. */ +#include #include #include #include +#include #include #include @@ -27,10 +29,13 @@ #include namespace graph_bag { -void FeatureTrackImage(const graph_localizer::FeatureTrackMap& feature_tracks, +namespace lc = localization_common; +namespace mc = msg_conversions; + +void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, const camera::CameraParameters& camera_params, cv::Mat& feature_track_image) { for (const auto& feature_track : feature_tracks) { - const auto& points = feature_track.second.points; + const auto& points = feature_track.second->points(); cv::Scalar color; if (points.size() <= 1) { // Red for single point tracks @@ -44,28 +49,40 @@ void FeatureTrackImage(const graph_localizer::FeatureTrackMap& feature_tracks, } // Draw track history - for (int i = 0; i < static_cast(points.size()) - 1; ++i) { - const auto distorted_previous_point = Distort(points[i].image_point, camera_params); - const auto distorted_current_point = Distort(points[i + 1].image_point, camera_params); - cv::circle(feature_track_image, distorted_current_point, 2 /* Radius*/, cv::Scalar(0, 255, 255), -1 /*Filled*/, - 8); - cv::line(feature_track_image, distorted_current_point, distorted_previous_point, color, 2, 8, 0); - } - // Account for single point tracks - if (points.size() == 1) { - cv::circle(feature_track_image, Distort(points[0].image_point, camera_params), 2 /* Radius*/, color, + if (points.size() > 1) { + for (auto point_it = points.begin(); point_it != std::prev(points.end()); ++point_it) { + const auto& point1 = point_it->second.image_point; + const auto& point2 = std::next(point_it)->second.image_point; + const auto distorted_previous_point = Distort(point1, camera_params); + const auto distorted_current_point = Distort(point2, camera_params); + cv::circle(feature_track_image, distorted_current_point, 2 /* Radius*/, cv::Scalar(0, 255, 255), -1 /*Filled*/, + 8); + cv::line(feature_track_image, distorted_current_point, distorted_previous_point, color, 2, 8, 0); + } + } else { + cv::circle(feature_track_image, Distort(points.cbegin()->second.image_point, camera_params), 2 /* Radius*/, color, -1 /*Filled*/, 8); } // Draw feature id at most recent point - cv::putText(feature_track_image, std::to_string(points[points.size() - 1].feature_id), - Distort(points[points.size() - 1].image_point, camera_params), CV_FONT_NORMAL, 0.4, + cv::putText(feature_track_image, std::to_string(points.crbegin()->second.feature_id), + Distort(points.crbegin()->second.image_point, camera_params), CV_FONT_NORMAL, 0.4, cv::Scalar(255, 0, 0)); } } +void MarkSmartFactorPoints(const std::vector smart_factors, + const camera::CameraParameters& camera_params, cv::Mat& feature_track_image) { + for (const auto smart_factor : smart_factors) { + const auto& point = smart_factor->measured().back(); + const auto distorted_point = Distort(point, camera_params); + cv::circle(feature_track_image, distorted_point, 15 /* Radius*/, cv::Scalar(200, 100, 0), -1 /*Filled*/, 8); + } +} + boost::optional CreateFeatureTrackImage(const sensor_msgs::ImageConstPtr& image_msg, - const graph_localizer::FeatureTrackMap& feature_tracks, - const camera::CameraParameters& camera_params) { + const graph_localizer::FeatureTrackIdMap& feature_tracks, + const camera::CameraParameters& camera_params, + const std::vector& smart_factors) { cv_bridge::CvImagePtr feature_track_image; try { feature_track_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::RGB8); @@ -75,6 +92,7 @@ boost::optional CreateFeatureTrackImage(const sensor_msgs } FeatureTrackImage(feature_tracks, camera_params, feature_track_image->image); + MarkSmartFactorPoints(smart_factors, camera_params, feature_track_image->image); return feature_track_image->toImageMsg(); } @@ -83,4 +101,37 @@ cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::Camera params.Convert(undistorted_point, &distorted_point); return cv::Point(distorted_point.x(), distorted_point.y()); } + +std::vector SmartFactors(const graph_localizer::GraphLocalizer& graph) { + std::vector smart_factors; + for (const auto factor : graph.graph_factors()) { + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + smart_factors.emplace_back(smart_factor); + } + } + return smart_factors; +} + +bool string_ends_with(const std::string& str, const std::string& ending) { + if (str.length() >= ending.length()) { + return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); + } else { + return false; + } +} + +void SaveImuBiasTesterPredictedStates(const std::vector& imu_bias_tester_predicted_states, + rosbag::Bag& bag) { + for (const auto& state : imu_bias_tester_predicted_states) { + geometry_msgs::PoseStamped pose_msg; + lc::PoseToMsg(state.pose(), pose_msg.pose); + lc::TimeToHeader(state.timestamp(), pose_msg.header); + SaveMsg(pose_msg, TOPIC_IMU_BIAS_TESTER_POSE, bag); + geometry_msgs::Vector3Stamped velocity_msg; + mc::VectorToMsg(state.velocity(), velocity_msg.vector); + lc::TimeToHeader(state.timestamp(), velocity_msg.header); + SaveMsg(velocity_msg, TOPIC_IMU_BIAS_TESTER_VELOCITY, bag); + } +} } // namespace graph_bag diff --git a/tools/graph_bag/tools/run_imu_bias_tester_adder.cc b/tools/graph_bag/tools/run_imu_bias_tester_adder.cc new file mode 100644 index 0000000000..b3118e147c --- /dev/null +++ b/tools/graph_bag/tools/run_imu_bias_tester_adder.cc @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include +#include + +#include +#include + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string robot_config_file; + std::string world; + po::options_description desc( + "Adds imu bias tester predictions to a new bag file using recorded localization states and imu msgs."); + desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Bagfile")( + "config-path,c", po::value()->required(), "Config path")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name"); + po::positional_options_description p; + p.add("bagfile", 1); + p.add("config-path", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + const std::string config_path = vm["config-path"].as(); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + boost::filesystem::path input_bag_path(input_bag); + boost::filesystem::path output_bag_path = + input_bag_path.parent_path() / + boost::filesystem::path(input_bag_path.stem().string() + "_with_imu_bias_tester_predictions.bag"); + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + /* config_reader::ConfigReader config; + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + }*/ + + graph_bag::ImuBiasTesterAdder imu_bias_tester_adder(input_bag, output_bag_path.string()); + imu_bias_tester_adder.AddPredictions(); +} diff --git a/tools/imu_bias_tester/src/imu_bias_tester.cc b/tools/imu_bias_tester/src/imu_bias_tester.cc index fa94de9d2f..b8dfbf85dc 100644 --- a/tools/imu_bias_tester/src/imu_bias_tester.cc +++ b/tools/imu_bias_tester/src/imu_bias_tester.cc @@ -52,7 +52,7 @@ void ImuBiasTester::IntegrateAndRemoveMeasurements(const lc::CombinedNavState& l auto pim = ii::Pim(lower_bound_state.bias(), pim_params()); // Reset pim after each integration since pim uses beginning orientation and velocity for // gravity integration and initial velocity integration. - for (auto measurement_it = measurements().cbegin(); + for (auto measurement_it = measurements().upper_bound(last_predicted_combined_nav_state_->timestamp()); measurement_it != measurements().cend() && measurement_it->first < upper_bound_timestamp; ++measurement_it) { pim.resetIntegrationAndSetBias(lower_bound_state.bias()); auto time = last_predicted_combined_nav_state_->timestamp(); diff --git a/tools/localization_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt index de1d5dec8e..f17c5b3475 100644 --- a/tools/localization_rviz_plugins/CMakeLists.txt +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -17,7 +17,8 @@ project(localization_rviz_plugins) -if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) +#TODO(rsoussan): enable this when build works again +if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS AND FALSE) # For Qt set(CMAKE_AUTOMOC ON) diff --git a/tools/localization_rviz_plugins/src/localization_graph_display.cc b/tools/localization_rviz_plugins/src/localization_graph_display.cc index ec197fe8b4..e576f5ea5a 100644 --- a/tools/localization_rviz_plugins/src/localization_graph_display.cc +++ b/tools/localization_rviz_plugins/src/localization_graph_display.cc @@ -43,6 +43,7 @@ #include "utilities.h" // NOLINT namespace localization_rviz_plugins { +namespace go = graph_optimizer; namespace lc = localization_common; LocalizationGraphDisplay::LocalizationGraphDisplay() { @@ -105,7 +106,7 @@ void LocalizationGraphDisplay::imageCallback(const sensor_msgs::ImageConstPtr& i img_buffer_.emplace(lc::TimeFromHeader(image_msg->header), image_msg); } -void LocalizationGraphDisplay::addOpticalFlowVisual(const graph_localizer::FeatureTrackMap& feature_tracks, +void LocalizationGraphDisplay::addOpticalFlowVisual(const graph_localizer::FeatureTrackIdMap& feature_tracks, const localization_common::Time latest_graph_time) { if (!publish_optical_flow_images_->getBool()) return; const auto img = getImage(latest_graph_time); @@ -223,7 +224,7 @@ void LocalizationGraphDisplay::addProjectionVisual(const gtsam::CameraSet*> loc_projection_factors, - const graph_localizer::GraphValues& graph_values) { + const graph_optimizer::GraphValues& graph_values) { if (!publish_loc_projection_factor_images_->getBool()) return; lc::Time latest_timestamp = std::numeric_limits::lowest(); for (const auto loc_projection_factor : loc_projection_factors) { @@ -323,7 +324,7 @@ void LocalizationGraphDisplay::addSmartFactorsProjectionVisual() { } void LocalizationGraphDisplay::addSmartFactorProjectionVisual(const SmartFactor& smart_factor, - const graph_localizer::GraphValues& graph_values) { + const graph_optimizer::GraphValues& graph_values) { if (!publish_smart_factor_images_->getBool()) return; std::vector images; for (const auto& key : smart_factor.keys()) { @@ -477,7 +478,7 @@ void LocalizationGraphDisplay::processMessage(const ff_msgs::LocalizationGraph:: if (latest_graph_localizer_->graph_values().LatestTimestamp()) addOpticalFlowVisual(latest_graph_localizer_->feature_tracks(), *(latest_graph_localizer_->graph_values().LatestTimestamp())); - for (const auto factor : latest_graph_localizer_->factor_graph()) { + for (const auto factor : latest_graph_localizer_->graph_factors()) { const auto smart_factor = dynamic_cast(factor.get()); if (smart_factor) { latest_smart_factors_.emplace_back(smart_factor); diff --git a/tools/localization_rviz_plugins/src/localization_graph_display.h b/tools/localization_rviz_plugins/src/localization_graph_display.h index ee108116d5..5e1fa067cb 100644 --- a/tools/localization_rviz_plugins/src/localization_graph_display.h +++ b/tools/localization_rviz_plugins/src/localization_graph_display.h @@ -78,13 +78,13 @@ class LocalizationGraphDisplay : public rviz::MessageFilterDisplay& cameras, const Camera::MeasurementVector& measurements, const gtsam::Point3& world_t_landmark, std::vector& images); void addLocProjectionVisual(const std::vector*> loc_projection_factors, - const graph_localizer::GraphValues& graph_values); - void addOpticalFlowVisual(const graph_localizer::FeatureTrackMap& feature_tracks, + const graph_optimizer::GraphValues& graph_values); + void addOpticalFlowVisual(const graph_localizer::FeatureTrackIdMap& feature_tracks, const localization_common::Time latest_graph_time); void clearImageBuffer(const localization_common::Time oldest_graph_time); sensor_msgs::ImageConstPtr getImage(const localization_common::Time time); void addSmartFactorProjectionVisual(const SmartFactor& smart_factor, - const graph_localizer::GraphValues& graph_values); + const graph_optimizer::GraphValues& graph_values); cv::Scalar textColor(const double val, const double green_threshold, const double yellow_threshold); std::vector> graph_pose_axes_; diff --git a/tools/localization_rviz_plugins/src/localization_graph_panel.cc b/tools/localization_rviz_plugins/src/localization_graph_panel.cc index e8765c69ef..82d5b30af5 100644 --- a/tools/localization_rviz_plugins/src/localization_graph_panel.cc +++ b/tools/localization_rviz_plugins/src/localization_graph_panel.cc @@ -208,7 +208,7 @@ void LocalizationGraphPanel::LocalizationGraphCallback(const ff_msgs::Localizati double pose_prior_error = 0; double velocity_prior_error = 0; double bias_prior_error = 0; - for (const auto factor : graph_localizer.factor_graph()) { + for (const auto factor : graph_localizer.graph_factors()) { const double error = factor->error(graph_localizer.graph_values().values()); total_error += error; const auto smart_factor = dynamic_cast(factor.get());