Move pair_to_index to avoid duplication

g2o/core/base_edge.h

@@ -31,6 +31,7 @@
 #include <climits>
 #include <type_traits>
 
+#include "g2o/config.h"  // IWYU pragma: keep
 #include "g2o/core/type_traits.h"
 #include "g2o/stuff/logger.h"
 #include "optimizable_graph.h"
@@ -56,6 +57,11 @@ struct QuadraticFormLock {
 };
 #endif
 
+// assumes i < j
+constexpr int pair_to_index(const int i, const int j) {
+  return ((j * (j - 1)) / 2) + i;
+}
+
 /**
  * Declaring the types for the error vector and the information matrix depending
  * on the size of the error function. In particular, the information matrix

g2o/core/base_fixed_sized_edge.h

@@ -60,13 +60,6 @@ inline std::array<bool, 0> createBoolArray<0>() {
   return std::array<bool, 0>();
 }
 
-// assumes i < j
-// duplication of internal::computeUpperTriangleIndex in
-// g2o/core/base_variable_sized_edge.hpp
-constexpr int pair_to_index(const int i, const int j) {
-  return j * (j - 1) / 2 + i;
-}
-
 /**
  * A trivial pair that has a constexpr c'tor.
  * std::pair in C++11 has not a constexpr c'tor.

g2o/core/base_variable_sized_edge.h

@@ -112,13 +112,6 @@ class BaseVariableSizedEdge : public BaseEdge<D, E> {
  public:
 };
 
-namespace internal {
-inline int computeUpperTriangleIndex(int i, int j) {
-  int elemsUpToCol = ((j - 1) * j) / 2;
-  return elemsUpToCol + i;
-}
-}  // namespace internal
-
 template <int D, typename E>
 void BaseVariableSizedEdge<D, E>::constructQuadraticForm() {
   if (this->robustKernel()) {
@@ -199,7 +192,7 @@ void BaseVariableSizedEdge<D, E>::linearizeOplus() {
 template <int D, typename E>
 void BaseVariableSizedEdge<D, E>::mapHessianMemory(double* d, int i, int j,
                                                    bool rowMajor) {
-  int idx = internal::computeUpperTriangleIndex(i, j);
+  const int idx = internal::pair_to_index(i, j);
   assert(idx < (int)hessian_.size());
   OptimizableGraph::Vertex* vi = vertexRaw(i);
   OptimizableGraph::Vertex* vj = vertexRaw(j);
@@ -241,42 +234,37 @@ void BaseVariableSizedEdge<D, E>::computeQuadraticForm(
     const InformationType& omega, const ErrorVector& weightedError) {
   for (size_t i = 0; i < vertices_.size(); ++i) {
     OptimizableGraph::Vertex* from = vertexRaw(i);
-    bool istatus = !(from->fixed());
-
-    if (istatus) {
-      const JacobianType& A = jacobianOplus_[i];
-
-      MatrixX AtO = A.transpose() * omega;
-      int fromDim = from->dimension();
-      assert(fromDim >= 0);
-      Eigen::Map<MatrixX> fromMap(from->hessianData(), fromDim, fromDim);
-      Eigen::Map<VectorX> fromB(from->bData(), fromDim);
-
-      // ii block in the hessian
-      {
-        internal::QuadraticFormLock lck(*from);
-        fromMap.noalias() += AtO * A;
-        fromB.noalias() += A.transpose() * weightedError;
-      }
+    if (from->fixed()) continue;
+
+    const JacobianType& A = jacobianOplus_[i];
+    MatrixX AtO = A.transpose() * omega;
+    int fromDim = from->dimension();
+    assert(fromDim >= 0);
+    Eigen::Map<MatrixX> fromMap(from->hessianData(), fromDim, fromDim);
+    Eigen::Map<VectorX> fromB(from->bData(), fromDim);
+
+    // ii block in the hessian
+    {
+      internal::QuadraticFormLock lck(*from);
+      fromMap.noalias() += AtO * A;
+      fromB.noalias() += A.transpose() * weightedError;
+    }
 
-      // compute the off-diagonal blocks ij for all j
-      for (size_t j = i + 1; j < vertices_.size(); ++j) {
-        OptimizableGraph::Vertex* to = vertexRaw(j);
-
-        bool jstatus = !(to->fixed());
-        if (jstatus) {
-          internal::QuadraticFormLock lck(*to);
-          const JacobianType& B = jacobianOplus_[j];
-          int idx = internal::computeUpperTriangleIndex(i, j);
-          assert(idx < (int)hessian_.size());
-          HessianHelper& hhelper = hessian_[idx];
-          if (hhelper
-                  .transposed) {  // we have to write to the block as transposed
-            hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
-          } else {
-            hhelper.matrix.noalias() += AtO * B;
-          }
-        }
+    // compute the off-diagonal blocks ij for all j
+    for (size_t j = i + 1; j < vertices_.size(); ++j) {
+      OptimizableGraph::Vertex* to = vertexRaw(j);
+      if (to->fixed()) continue;
+
+      internal::QuadraticFormLock lck(*to);
+      const JacobianType& B = jacobianOplus_[j];
+      const int idx = internal::pair_to_index(i, j);
+      assert(idx < (int)hessian_.size());
+      HessianHelper& hhelper = hessian_[idx];
+      if (hhelper.transposed) {
+        // we have to write to the block as transposed
+        hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
+      } else {
+        hhelper.matrix.noalias() += AtO * B;
       }
     }
   }

g2o/core/openmp_mutex.h

@@ -27,7 +27,7 @@
 #ifndef G2O_OPENMP_MUTEX
 #define G2O_OPENMP_MUTEX
 
-#include "g2o/config.h"
+#include "g2o/config.h"  // IWYU pragma: keep
 
 #ifdef G2O_OPENMP
 #include <omp.h>

g2o/core/optimizable_graph.cpp

@@ -39,6 +39,7 @@
 
 #include "cache.h"
 #include "factory.h"
+#include "g2o/config.h"  // IWYU pragma: keep
 #include "g2o/core/abstract_graph.h"
 #include "g2o/core/eigen_types.h"
 #include "g2o/core/hyper_graph.h"
@@ -65,11 +66,10 @@ void saveUserData(AbstractGraph::AbstractGraphElement& graph_element,
   // write the data packet for the vertex
   for (const auto& d : data) {
     const std::string tag = factory->tag(d.get());
-    if (!tag.empty()) {
-      std::stringstream buffer;
-      d->write(buffer);
-      graph_element.data.emplace_back(tag, buffer.str());
-    }
+    if (tag.empty()) continue;
+    std::stringstream buffer;
+    d->write(buffer);
+    graph_element.data.emplace_back(tag, buffer.str());
   }
 }
 
@@ -247,13 +247,6 @@ std::shared_ptr<const OptimizableGraph::Vertex> OptimizableGraph::vertex(
       HyperGraph::vertex(id));
 }
 
-// bool OptimizableGraph::addEdge(const std::shared_ptr<HyperGraph::Edge>& e_) {
-//   std::shared_ptr<OptimizableGraph::Edge> e =
-//   dynamic_pointer_cast<OptimizableGraph::Edge>(e_); assert(e && "Edge does
-//   not inherit from OptimizableGraph::Edge"); if (!e) return false; return
-//   addEdge(e);
-// }
-
 bool OptimizableGraph::setEdgeVertex(
     const std::shared_ptr<HyperGraph::Edge>& e, int pos,
     const std::shared_ptr<HyperGraph::Vertex>& v) {