Fix test and address review comments

Author: dellaert

gtsam/base/tests/testPriorityScheduler.cpp

@@ -62,15 +62,19 @@ TEST(PriorityScheduler, VoidPriorityOrderingSingleWorker) {
   scheduler.waitForAllTasks();
   EXPECT_LONGS_EQUAL(3, executionOrder.size());
   // With a single worker, tasks are not preempted: the first scheduled task may
-  // start running before later (higher-priority) tasks are submitted. Once all
-  // tasks are enqueued, remaining work should respect priority order.
+  // start running before later (higher-priority) tasks are submitted. Also,
+  // since the third task is submitted last, the worker may run the first two
+  // tasks before the third is enqueued.
   const bool strictPriority =
       (executionOrder.at(0) == 1 && executionOrder.at(1) == 3 &&
        executionOrder.at(2) == 5);
   const bool firstTaskRanImmediately =
       (executionOrder.at(0) == 5 && executionOrder.at(1) == 1 &&
        executionOrder.at(2) == 3);
-  EXPECT(strictPriority || firstTaskRanImmediately);
+  const bool thirdTaskEnqueuedLate =
+      (executionOrder.at(0) == 1 && executionOrder.at(1) == 5 &&
+       executionOrder.at(2) == 3);
+  EXPECT(strictPriority || firstTaskRanImmediately || thirdTaskEnqueuedLate);
 }
 
 /* ************************************************************************* */

gtsam/linear/MultifrontalClique.cpp

@@ -326,9 +326,7 @@ void MultifrontalClique::updateParentSbm(
     SymmetricBlockMatrix& parentSbm) const {
   if (useQR()) {
     // Accumulate separator (and RHS) normal equations (S^T S) into the parent.
-    assert(RSdReady_);
-    assert(RSd_.firstBlock() == 0);
-    assert(parent.sbm_.nBlocks() > 0);
+    assert(RSdReady_ && RSd_.firstBlock() == 0);
     const DenseIndex nfBlocks = static_cast<DenseIndex>(numFrontals());
     RSd_.firstBlock() = nfBlocks;
     parentSbm.updateFromOuterProductBlocks(RSd_, parentIndices_);
@@ -365,7 +363,7 @@ void MultifrontalClique::gatherUpdatesParallel(size_t numThreads) {
         auto& local = locals.local();  // Thread-local SBM.
         for (size_t i = range.begin(); i < range.end(); ++i) {
           const auto& child = children[i];
-          if (!child) continue;
+          assert(child);
           child->updateParentSbm(
               local);  // No locking: each thread writes its own SBM.
         }
@@ -392,7 +390,7 @@ void MultifrontalClique::gatherUpdatesParallel(size_t numThreads) {
       auto& local = locals[t];
       for (size_t i = start; i < end; ++i) {
         const auto& child = children[i];
-        if (!child) continue;
+        assert(child);
         child->updateParentSbm(
             local);  // No locking: each thread writes its own SBM.
       }