Issue258 add fixed size optimizer #261

jakemclaughlin6 · 2021-12-06T18:01:50Z

No description provided.

jakemclaughlin6 · 2022-01-05T19:12:38Z

@svwilliams Can this be reviewed soon, not sure if you would like some tests written for it since the other optimizers dont have any either, but the changes I made to timestamp tracker have been tested and I've also been using this optimizer for my own code and it seems to work well.

svwilliams · 2022-01-05T19:26:14Z

It may be a couple of weeks before I can take a look. But I'll do my best to get to it sooner.

jakemclaughlin6 · 2022-02-02T20:15:16Z

This PR implements a fixed size window optimizer where the window is determined by the number of "states" in it. The number of states are determined by the total # of unique timestamps exist in the graph.

Another addition from this PR is the fix to the following issue: #248. It would seem that what was occurring in a fringe edge case was a sensor model was sending a transaction and adding constraints to variables that were already chosen to be marginalized out in the previous cycle, the fix involved just checking for these added constraints and removing them. I am curious if this fix is suitable or if there should be another method to actually re marginalize the constraints that have been added @svwilliams.

Lastly, a duplicate of the reset service was implemented but as a message callback, this is for the case where a system would like to call a reset however only knows when it would like to reset in its own message callbacks. Calling the service inside the sensors callback created a deadlock, making a message callback version allows for the sensor model to ask for a reset and entrust that all onStart() and onStop() functions perform the required logic to reset the system

svwilliams

My big concern is in the VariableStampIndex. It seems to me that timestamps can be removed while they are still in use by other variables.

Additionally, there is a bit of cleanup needed in CMakelists and numerous linter errors that need to be fixed to conform to ROS coding conventions:

/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h:195:  At least two spaces is best between code and comments  [whitespace/comments] [2]
/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h:195:  Add #include <set> for set<>  [build/include_what_you_use] [4]
/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h:188:  At least two spaces is best between code and comments  [whitespace/comments] [2]
/fuse_optimizers/src/fixed_size_smoother.cpp:89:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/fixed_size_smoother.cpp:89:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/fixed_size_smoother.cpp:89:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/fixed_size_smoother.cpp:89:  Using C-style cast.  Use static_cast<int>(...) instead  [readability/casting] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:50:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:50:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:50:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:58:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:58:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:58:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:62:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:64:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:64:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:65:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:65:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:65:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:67:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:67:  Missing space before ( in for(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:67:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:113:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:113:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:113:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:139:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/variable_stamp_index.cpp:139:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:139:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/variable_stamp_index.cpp:66:  Add #include <set> for set<>  [build/include_what_you_use] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:110:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:180:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:180:  Missing space around colon in range-based for loop  [whitespace/forcolon] [2]
/fuse_optimizers/src/windowed_optimizer.cpp:180:  Missing space before ( in for(  [whitespace/parens] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:180:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:181:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:181:  Missing space around colon in range-based for loop  [whitespace/forcolon] [2]
/fuse_optimizers/src/windowed_optimizer.cpp:181:  Missing space before ( in for(  [whitespace/parens] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:181:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:182:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:183:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:190:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:190:  Missing space before ( in if(  [whitespace/parens] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:190:  Missing space before {  [whitespace/braces] [5]
/fuse_optimizers/src/windowed_optimizer.cpp:192:  when starting a new scope, { should be on a line by itself  [whitespace/braces] [4]
/fuse_optimizers/src/windowed_optimizer.cpp:192:  Missing space around colon in range-based for loop  [whitespace/forcolon] [2]

svwilliams · 2022-02-06T22:04:39Z

fuse_optimizers/CMakeLists.txt

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(fuse_optimizers)

+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")


You should never set the CMAKE_CXX_FLAGS directly in a catkin package. If you are using catkin_make, then a single makefile is created for the entire workspace. Modifying CXX_FLAGS like this pollutes the entire workspace. It's better to add the required compile flags on a per-target basis.

Suggested change

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")

And you should find_package(OpenMP) before assuming OpenMP is available.

And then add something like this to the library config

target_link_libraries(${PROJECT_NAME} ${OpenMP_CXX_LIBRARIES} ) target_compile_options(${PROJECT_NAME} PRIVATE ${OpenMP_CXX_FLAGS} )

svwilliams · 2022-02-06T22:16:36Z

fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Locus Robotics


Suggested change

* Copyright (c) 2019, Locus Robotics

* Copyright (c) 2022, Locus Robotics

svwilliams · 2022-02-06T22:20:00Z

fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h

+namespace fuse_optimizers
+{
+/**
+ * @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time


Suggested change

* @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time

* @brief A fixed-size smoother implementation that marginalizes out variables that are older than a defined size

Or maybe?

Suggested change

* @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time

* @brief A fixed-size smoother implementation that marginalizes out variables that are older than a defined buffer length

Also, it looks like find&replace got away from you. Review all of the comments and make sure Size is properly capitalized and makes sense in the sentence.

svwilliams · 2022-02-06T22:21:03Z

fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h

+ *  (1) new variables and constraints are added to the graph
+ *  (2) the augmented graph is optimized and the variable values are updated
+ *  (3) all motion models, sensors, and publishers are notified of the updated graph
+ *  (4) all variables older than "current time - Size duration" are marginalized out.


I'm guessing this statement should be revised

svwilliams · 2022-02-06T22:22:31Z

fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h

+  SMART_PTR_DEFINITIONS(FixedSizeSmootherParams);
+
+  /**
+   * @brief The duration of the smoothing window in seconds


This statement needs to be revised

svwilliams · 2022-02-06T23:28:15Z

fuse_optimizers/src/variable_stamp_index.cpp

+    // remove from unique stamps
+    auto stamp = stamped_index_[variable_uuid];
+    if(unique_stamps_.find(stamp) != unique_stamps_.end()){
+      unique_stamps_.erase(stamp);
+    }


This comment is important:
Multiple variables may be associated with the same timestamp. The motion model in the fuse_models package uses position, orientation, linear velocity, angular velocity, and linear acceleration variables all at the exact same timestamp. Just because you remove the linear acceleration variable doesn't mean that the other variables are also removed. It feels like you really need to keep track of the individual variables associated with each timestamp, and only remove a timestamp once all variables have been removed.
Some sort of structure like:

using StampToVariablesMap = std::map<ros::Time, std::unordered_set<fuse_core::UUID>>; StampToVariablesMap unique_stamps_;

svwilliams · 2022-02-06T23:42:02Z

fuse_optimizers/src/windowed_optimizer.cpp

+      // check if new transaction has added constraints to variables that are to be marginalized
+      std::vector<fuse_core::UUID> faulty_constraints;
+      for(auto& c: new_transaction->addedConstraints()){
+        for(auto var_uuid: c.variables()){
+          for (auto marginal_uuid : marginal_transaction_.removedVariables()) {
+            if (var_uuid == marginal_uuid) {
+              faulty_constraints.push_back(c.uuid());
+              break;
+            }
+          }
+        }
+      }
+      if(faulty_constraints.size() > 0){
+        ROS_WARN_STREAM("Removing invalid constraints.");
+        for(auto& faulty_constraint: faulty_constraints){
+          new_transaction->removeConstraint(faulty_constraint);
+        }
+      }


Interesting. There is indeed a timing problem. At the end of Loop1 we compute a set of variables to marginalize. But those variables are not removed until the graph is updated in Loop2. The sensor models have no way of knowing a variable was removed in Loop1 until after Loop2 has been processed and the updated graph is sent back to the sensor models. So a sensor model can very reasonably send new constraints involving variables that should have been removed.

I originally implemented it this way to avoid optimizing the graph twice. Once when new constraints are added, and a second time to remove the marginalized variables. In retrospect, marginalizing variables from the graph should not alter the optimized values of the remaining variables. So I could just remove the marginalized variables at the end of the loop without reoptimizing. But I wouldn't want to make that change without a lot of testing first.

For now, your fix should be valid. Thanks.

svwilliams · 2022-02-06T23:43:21Z

fuse_optimizers/src/windowed_optimizer.cpp

+  // Tell all the plugins to stop
+  stopPlugins();
+  // Reset the optimizer state
+  {
+    std::lock_guard<std::mutex> lock(optimization_requested_mutex_);
+    optimization_request_ = false;
+  }
+  started_ = false;
+  ignited_ = false;
+  setStartTime(ros::Time(0, 0));
+  // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the
+  //         pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to
+  //         prevent the possibility of deadlocks.
+  {
+    std::lock_guard<std::mutex> lock(optimization_mutex_);
+    // Clear all pending transactions
+    {
+      std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
+      pending_transactions_.clear();
+    }
+    // Clear the graph and marginal tracking states
+    graph_->clear();
+    marginal_transaction_ = fuse_core::Transaction();
+  }
+  // Perform any required reset operations for derived classes
+  onReset();
+  // Tell all the plugins to start
+  startPlugins();
+  // Test for auto-start
+  autostart();
+  return;


This is identical to the code in the resetServiceCallback(). Can you pull this out into a reset() function, and call that function from both resetServiceCallback() and resetMessageCallback()?

svwilliams · 2022-02-06T23:46:58Z

fuse_optimizers/test/test_variable_stamp_index.cpp

+  int N = 10;
+
+  for(int i = 0; i < N; i++){
+    auto x = StampedVariable::make_shared(ros::Time(i, 0));
+    auto transaction = fuse_core::Transaction();
+    transaction.addVariable(x);
+    index.addNewTransaction(transaction);
+  }
+
+  // Verify the number of unique stamps
+  EXPECT_EQ((size_t)N, index.numStates());


If you use unsigned types for N and i, you won't need to typecast anything.

Suggested change

int N = 10;

for(int i = 0; i < N; i++){

auto x = StampedVariable::make_shared(ros::Time(i, 0));

auto transaction = fuse_core::Transaction();

transaction.addVariable(x);

index.addNewTransaction(transaction);

}

// Verify the number of unique stamps

EXPECT_EQ((size_t)N, index.numStates());

unsigned int N = 10u;

for(auto i = 0u; i < N; i++)

{

auto x = StampedVariable::make_shared(ros::Time(i, 0));

auto transaction = fuse_core::Transaction();

transaction.addVariable(x);

index.addNewTransaction(transaction);

}

// Verify the number of unique stamps

EXPECT_EQ(N, index.numStates());

svwilliams · 2022-02-06T23:48:25Z

fuse_optimizers/test/test_variable_stamp_index.cpp

+  int N = 10;
+  for(int i = 0; i < N; i++){
+    auto x = StampedVariable::make_shared(ros::Time(i, 0));
+    auto transaction = fuse_core::Transaction();
+    transaction.addVariable(x);
+    index.addNewTransaction(transaction);
+  }
+
+  // Verify the stamps of the states using ithStamp function
+  for(int i = 0; i < N; i++){
+    EXPECT_EQ(ros::Time(i, 0), index.ithStamp((size_t)i));  
+  }


Same here

Suggested change

int N = 10;

for(int i = 0; i < N; i++){

auto x = StampedVariable::make_shared(ros::Time(i, 0));

auto transaction = fuse_core::Transaction();

transaction.addVariable(x);

index.addNewTransaction(transaction);

}

// Verify the stamps of the states using ithStamp function

for(int i = 0; i < N; i++){

EXPECT_EQ(ros::Time(i, 0), index.ithStamp((size_t)i));

}

unsigned int N = 10u;

for(auto i = 0u; i < N; i++)

{

auto x = StampedVariable::make_shared(ros::Time(i, 0));

auto transaction = fuse_core::Transaction();

transaction.addVariable(x);

index.addNewTransaction(transaction);

}

// Verify the stamps of the states using ithStamp function

for(auto i = 0u; i < N; i++)

{

EXPECT_EQ(ros::Time(i, 0), index.ithStamp(i));

}

Fix invalid constraints

…volved threads, added mutex guards to the fixed-lag smoother. Testing is still needed.

jakemclaughlin6 mentioned this pull request Dec 6, 2021

Sliding window optimizer #258

Open

svwilliams requested changes Feb 6, 2022

View reviewed changes

jakemclaughlin6 requested a review from svwilliams February 23, 2022 17:27

jakemclaughlin6 and others added 16 commits November 20, 2022 16:49

Implement check for adding constraints to marginalized variables

bb77ad7

Add helper to landmakr variable

6a0ea4e

Merge pull request #1 from jakemclaughlin6/fix_invalid_constraints

a1150e1

Fix invalid constraints

WIP

477ee28

Cleaned up headers, fixed parameter inheritance issues, documented in…

c3ff048

…volved threads, added mutex guards to the fixed-lag smoother. Testing is still needed.

Initial fixed size smoother implementation

e37da70

Initial fixed size smoother implementation

25c3958

Add fixed size smoother node

bc5c21e

Remove temp compile flag

1d88752

Remove vscode folder

68b17d8

fix compile errors

febdb5f

Fix marginalization bug

caeb278

Messy fix to marginalization issue

fce9dbd

Add reset message callback for resets called within other callbacks

002d6d1

Address PR comments

157cd2d

formatting

70c03e5

jakemclaughlin6 force-pushed the issue258-add-fixed-size-optimizer branch from 74876a6 to 70c03e5 Compare January 18, 2023 21:27

jakemclaughlin6 added 2 commits January 18, 2023 16:35

formatting

33faaaf

Remove marginalization fix

5a3e36f

jakemclaughlin6 mentioned this pull request Jan 18, 2023

Issue258 add fixed size optimizer #308

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Issue258 add fixed size optimizer #261

Issue258 add fixed size optimizer #261

jakemclaughlin6 commented Dec 6, 2021

jakemclaughlin6 commented Jan 5, 2022

svwilliams commented Jan 5, 2022

jakemclaughlin6 commented Feb 2, 2022

svwilliams left a comment

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

svwilliams Feb 6, 2022

	* Copyright (c) 2019, Locus Robotics
	* Copyright (c) 2022, Locus Robotics

	* @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time
	* @brief A fixed-size smoother implementation that marginalizes out variables that are older than a defined size

Issue258 add fixed size optimizer #261

Are you sure you want to change the base?

Issue258 add fixed size optimizer #261

Conversation

jakemclaughlin6 commented Dec 6, 2021

jakemclaughlin6 commented Jan 5, 2022

svwilliams commented Jan 5, 2022

jakemclaughlin6 commented Feb 2, 2022

svwilliams left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment