Reduce duplication in slam2d_addons

RainerKuemmerle · Jan 1, 2025 · 8443f08 · 8443f08
1 parent 6a05a33
commit 8443f08
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 52 deletions.
diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d_line.cpp
@@ -28,37 +28,32 @@
 
 #include <cmath>
 
+#include "g2o/core/eigen_types.h"
+
 namespace g2o {
 
 void EdgeSE2Segment2DLine::computeError() {
-  const VertexSE2* v1 = vertexXnRaw<0>();
-  const VertexSegment2D* l2 = vertexXnRaw<1>();
-  SE2 iEst = v1->estimate().inverse();
-  Vector2 predP1 = iEst * l2->estimateP1();
-  Vector2 predP2 = iEst * l2->estimateP2();
-  Vector2 dP = predP2 - predP1;
-  Vector2 normal(dP.y(), -dP.x());
-  normal.normalize();
-  Vector2 prediction(std::atan2(normal.y(), normal.x()),
-                     predP1.dot(normal) * .5 + predP2.dot(normal) * .5);
-
-  error_ = prediction - measurement_;
+  const Vector2 prediction_vec = prediction();
+  error_ = prediction_vec - measurement_;
   error_[0] = normalize_theta(error_[0]);
 }
 
 bool EdgeSE2Segment2DLine::setMeasurementFromState() {
+  measurement_ = prediction();
+  return true;
+}
+
+Vector2 EdgeSE2Segment2DLine::prediction() const {
   const VertexSE2* v1 = vertexXnRaw<0>();
   const VertexSegment2D* l2 = vertexXnRaw<1>();
-  SE2 iEst = v1->estimate().inverse();
-  Vector2 predP1 = iEst * l2->estimateP1();
-  Vector2 predP2 = iEst * l2->estimateP2();
-  Vector2 dP = predP2 - predP1;
-  Vector2 normal(dP.y(), -dP.x());
-  normal.normalize();
+  const SE2 iEst = v1->estimate().inverse();
+  const Vector2 predP1 = iEst * l2->estimateP1();
+  const Vector2 predP2 = iEst * l2->estimateP2();
+  const Vector2 dP = predP2 - predP1;
+  const Vector2 normal = Vector2(dP.y(), -dP.x()).normalized();
   Vector2 prediction(std::atan2(normal.y(), normal.x()),
-                     predP1.dot(normal) * .5 + predP2.dot(normal) * .5);
-  measurement_ = prediction;
-  return true;
+                     (predP1.dot(normal) * .5) + (predP2.dot(normal) * .5));
+  return prediction;
 }
 
 }  // namespace g2o
diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h
@@ -49,6 +49,9 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLine
   void computeError() override;
 
   bool setMeasurementFromState() override;
+
+ protected:
+  [[nodiscard]] Vector2 prediction() const;
 };
 
 }  // namespace g2o

diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.cpp
@@ -28,40 +28,33 @@
 
 #include <cmath>
 
+#include "g2o/core/eigen_types.h"
+
 namespace g2o {
 
 void EdgeSE2Segment2DPointLine::computeError() {
-  const VertexSE2* v1 = vertexXnRaw<0>();
-  const VertexSegment2D* l2 = vertexXnRaw<1>();
-  SE2 iEst = v1->estimate().inverse();
-  Vector2 predP1 = iEst * l2->estimateP1();
-  Vector2 predP2 = iEst * l2->estimateP2();
-  Vector2 dP = predP2 - predP1;
-  Vector2 normal(dP.y(), -dP.x());
-  normal.normalize();
-  Vector3 prediction;
-  prediction[2] = std::atan2(normal.y(), normal.x());
-  Eigen::Map<Vector2> pt(prediction.data());
-  pt = (pointNum_ == 0) ? predP1 : predP2;
-  error_ = prediction - measurement_;
+  const Vector3 prediction_vec = prediction();
+  error_ = prediction_vec - measurement_;
   error_[2] = normalize_theta(error_[2]);
 }
 
 bool EdgeSE2Segment2DPointLine::setMeasurementFromState() {
+  setMeasurement(prediction());
+  return true;
+}
+
+[[nodiscard]] Vector3 EdgeSE2Segment2DPointLine::prediction() const {
   const VertexSE2* v1 = vertexXnRaw<0>();
   const VertexSegment2D* l2 = vertexXnRaw<1>();
-  SE2 iEst = v1->estimate().inverse();
-  Vector2 predP1 = iEst * l2->estimateP1();
-  Vector2 predP2 = iEst * l2->estimateP2();
-  Vector2 dP = predP2 - predP1;
-  Vector2 normal(dP.y(), -dP.x());
-  normal.normalize();
+  const SE2 iEst = v1->estimate().inverse();
+  const Vector2 predP1 = iEst * l2->estimateP1();
+  const Vector2 predP2 = iEst * l2->estimateP2();
+  const Vector2 dP = predP2 - predP1;
+  const Vector2 normal = Vector2(dP.y(), -dP.x()).normalized();
   Vector3 prediction;
-  prediction[2] = std::atan2(normal.y(), normal.x());
-  Eigen::Map<Vector2> pt(prediction.data());
-  pt = (pointNum_ == 0) ? predP1 : predP2;
-  setMeasurement(prediction);
-  return true;
+  prediction << ((pointNum_ == 0) ? predP1 : predP2),
+      std::atan2(normal.y(), normal.x());
+  return prediction;
 }
 
 }  // namespace g2o
diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h
@@ -47,10 +47,7 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DPointLine
   }
 
   void setTheta(double t) { measurement_[2] = t; }
-  void setPoint(const Vector2& p_) {
-    Eigen::Map<Vector2> p(measurement_.data());
-    p = p_;
-  }
+  void setPoint(const Vector2& p) { measurement_.head<2>() = p; }
 
   [[nodiscard]] int pointNum() const { return pointNum_; }
   void setPointNum(int pn) { pointNum_ = pn; }
@@ -62,9 +59,7 @@ class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DPointLine
  protected:
   int pointNum_ = 0;
 
-  /* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */
-  /*       virtual void linearizeOplus(); */
-  /* #endif */
+  [[nodiscard]] Vector3 prediction() const;
 };
 
 }  // namespace g2o