Store rowmajor flag for each Hessian block

The BaseFixedSizedEdge template needs to store for each Hessian block if
the respective block is transposed or not. Before a single boolean was
used which means that for an edge all blocks had to be on the upper
triangular block of the Hessian.

fixes RainerKuemmerle#504

g2o/core/base_fixed_sized_edge.h

@@ -27,6 +27,7 @@
 #ifndef G2O_BASE_FIXED_SIZED_EDGE_H
 #define G2O_BASE_FIXED_SIZED_EDGE_H
 
+#include <array>
 #include <iostream>
 #include <limits>
 #include <utility>
@@ -46,6 +47,18 @@
 namespace g2o {
 
 namespace internal {
+
+// creating a bool array
+template <int K>
+std::array<bool, K> createBoolArray() {
+  std::array<bool, K> aux = {false};
+  return aux;
+}
+template <>
+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; }
@@ -186,10 +199,11 @@ class BaseFixedSizedEdge : public BaseEdge<D, E> {
       typename HessianTupleType<std::make_index_sequence<_nr_of_vertex_pairs>>::type;
   using HessianTupleTransposed =
       typename HessianTupleType<std::make_index_sequence<_nr_of_vertex_pairs>>::typeTransposed;
+  using HessianRowMajorStorage = std::array<bool, _nr_of_vertex_pairs>;
 
   BaseFixedSizedEdge()
       : BaseEdge<D, E>(),
-        _hessianRowMajor(false),
+         _hessianRowMajor(internal::createBoolArray<_nr_of_vertex_pairs>()),
         _hessianTuple(internal::createHessianMaps(_hessianTuple)),
         _hessianTupleTransposed(internal::createHessianMaps(_hessianTupleTransposed)),
         _jacobianOplus({nullptr, D, VertexTypes::Dimension}...) {
@@ -266,7 +280,7 @@ class BaseFixedSizedEdge : public BaseEdge<D, E> {
   using BaseEdge<D, E>::_vertices;
   using BaseEdge<D, E>::_dimension;
 
-  bool _hessianRowMajor;
+  HessianRowMajorStorage _hessianRowMajor;
   HessianTuple _hessianTuple;
   HessianTupleTransposed _hessianTupleTransposed;
   std::tuple<JacobianType<D, VertexTypes::Dimension>...> _jacobianOplus;

g2o/core/base_fixed_sized_edge.hpp

@@ -88,13 +88,14 @@ void BaseFixedSizedEdge<D, E, VertexTypes...>::constructOffDiagonalQuadraticForm
     const AtOType& AtO) {
   constexpr auto fromId = N;
   constexpr auto toId = N + M + 1;
+  assert(fromId < toId && "Index mixed up");
   auto to = vertexXn<toId>();
   if (!to->fixed()) {
     const auto& B = std::get<toId>(_jacobianOplus);
     constexpr auto K = internal::pair_to_index(fromId, toId);
     internal::QuadraticFormLock lck(*to);
     (void)lck;
-    if (_hessianRowMajor) {  // we have to write to the block as transposed
+    if (_hessianRowMajor[K]) {  // we have to write to the block as transposed
       auto& hessianTransposed = std::get<K>(_hessianTupleTransposed);
       hessianTransposed.noalias() += B.transpose() * AtO.transpose();
     } else {
@@ -217,14 +218,14 @@ struct MapHessianMemoryK {
 template <int D, typename E, typename... VertexTypes>
 void BaseFixedSizedEdge<D, E, VertexTypes...>::mapHessianMemory(number_t* d, int i, int j,
                                                                 bool rowMajor) {
+  assert(i < j && "index assumption violated");
   // get the size of the vertices
   int vi_dim = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(i))->dimension();
   int vj_dim = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(j))->dimension();
-  _hessianRowMajor = rowMajor;
+  int k = internal::pair_to_index(i, j);
+  _hessianRowMajor[k] = rowMajor;
   if (rowMajor)
-    tuple_apply_i(MapHessianMemoryK{d, vj_dim, vi_dim}, _hessianTupleTransposed,
-                  internal::pair_to_index(i, j));
+    tuple_apply_i(MapHessianMemoryK{d, vj_dim, vi_dim}, _hessianTupleTransposed, k);
   else
-    tuple_apply_i(MapHessianMemoryK{d, vi_dim, vj_dim}, _hessianTuple,
-                  internal::pair_to_index(i, j));
+    tuple_apply_i(MapHessianMemoryK{d, vi_dim, vj_dim}, _hessianTuple, k);
 }