Merge remote-tracking branch 'origin/master' into pymem

RainerKuemmerle · Jul 20, 2024 · f51c609 · f51c609
2 parents 16a5aa1 + f535886
commit f51c609
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 10 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -32,7 +32,7 @@ jobs:
 
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
             fetch-depth: 25
 
@@ -68,7 +68,7 @@ jobs:
 
       - name: Install dependencies on macOS
         if: runner.os == 'macOS'
-        run: brew install cmake eigen
+        run: brew install cmake eigen coreutils
 
       - name: Configure ccache for macOS
         if: runner.os == 'macOS'
@@ -84,6 +84,7 @@ jobs:
 
       - name: Configure CMake
         run: |
+          echo "number of cores: $(nproc) $(nproc --all)"
           cmake --version
           cmake -B ${{github.workspace}}/build -DBUILD_UNITTESTS=ON -DCMAKE_BUILD_TYPE=${{matrix.config.build_type}} -DCMAKE_C_COMPILER_LAUNCHER=ccache -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
 
@@ -96,7 +97,7 @@ jobs:
           env | sort
 
       - name: Build
-        run: cmake --build ${{github.workspace}}/build --config ${{matrix.config.build_type}} -j 2
+        run: cmake --build ${{github.workspace}}/build --config ${{matrix.config.build_type}} -j $(nproc)
 
       - name: Test
         working-directory: ${{github.workspace}}/build

diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml
@@ -25,7 +25,7 @@ jobs:
           }
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
             fetch-depth: 25
 
@@ -55,11 +55,12 @@ jobs:
 
       - name: Build Introspection
         run: |
+          echo "Number of processors: $Env:NUMBER_OF_PROCESSORS"
           echo "g2o config.h"
           type ${{github.workspace}}/build/g2o/config.h
 
       - name: Build
-        run: cmake --build ${{github.workspace}}/build -j 2 --config ${{matrix.config.build_type}}
+        run: cmake --build ${{github.workspace}}/build -j $Env:NUMBER_OF_PROCESSORS --config ${{matrix.config.build_type}}
 
       - name: Test
         run: |

diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp b/g2o/types/slam2d/edge_se2_pointxy_bearing.cpp
@@ -56,6 +56,26 @@ void EdgeSE2PointXYBearing::initialEstimate(
   l2->setEstimate(t * vr);
 }
 
+#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
+void EdgeSE2PointXYBearing::linearizeOplus() {
+  const VertexSE2* vi = vertexXnRaw<0>();
+  const VertexPointXY* vj = vertexXnRaw<1>();
+  const double& x1 = vi->estimate().translation()[0];
+  const double& y1 = vi->estimate().translation()[1];
+  const double& x2 = vj->estimate()[0];
+  const double& y2 = vj->estimate()[1];
+
+  double aux = (std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2));
+
+  jacobianOplusXi_(0, 0) = (y1 - y2) / aux;
+  jacobianOplusXi_(0, 1) = (x2 - x1) / aux;
+  jacobianOplusXi_(0, 2) = 1;
+
+  jacobianOplusXj_(0, 0) = (y2 - y1) / aux;
+  jacobianOplusXj_(0, 1) = (x1 - x2) / aux;
+}
+#endif
+
 #ifdef G2O_HAVE_OPENGL
 EdgeSE2PointXYBearingDrawAction::EdgeSE2PointXYBearingDrawAction()
     : DrawAction(typeid(EdgeSE2PointXYBearing).name()) {}

diff --git a/g2o/types/slam2d/edge_se2_pointxy_bearing.h b/g2o/types/slam2d/edge_se2_pointxy_bearing.h
@@ -70,6 +70,10 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearing
   }
   void initialEstimate(const OptimizableGraph::VertexSet& from,
                        OptimizableGraph::Vertex* to) override;
+
+#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
+  void linearizeOplus() override;
+#endif
 };
 
 #ifdef G2O_HAVE_OPENGL

diff --git a/unit_test/slam2d/jacobians_slam2d.cpp b/unit_test/slam2d/jacobians_slam2d.cpp
@@ -157,10 +157,18 @@ TEST(Slam2D, EdgeSE2PointXYBearingJacobian) {
   numericJacobianWorkspace.allocate();
 
   for (int k = 0; k < 10000; ++k) {
-    v1->setEstimate(randomSE2());
-    v2->setEstimate(Eigen::Vector2d::Random());
-    e.setMeasurement(g2o::Sampler::uniformRand(0., 1.) * M_PI);
-
-    evaluateJacobian(e, jacobianWorkspace, numericJacobianWorkspace);
+    /* Generate random estimate states, but don't evaluate those that are too
+     * close to error function singularity. */
+    do {
+      v1->setEstimate(randomSE2());
+      v2->setEstimate(Eigen::Vector2d::Random());
+      e.setMeasurement(g2o::Sampler::uniformRand(-1., 1.) * M_PI);
+    } while ((v1->estimate().inverse() * v2->estimate()).norm() < 1e-6);
+
+    /* Note a larger tolerance versus the default of 1e-6 must be used due to
+     * poor behaviour of the numerical difference function that is used to
+     * provide golden data. */
+    evaluateJacobian(e, jacobianWorkspace, numericJacobianWorkspace,
+                     [](const double, const double) { return 1e-5; });
   }
 }