Fix issues spotted by codacy

RainerKuemmerle · Jun 9, 2024 · 3e5bc87 · 3e5bc87
1 parent 8b2cb38
commit 3e5bc87
Show file tree

Hide file tree

Showing 15 changed files with 70 additions and 87 deletions.
diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h
@@ -566,7 +566,7 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph {
     int dimension_ = -1;
     int level_ = 0;
     std::shared_ptr<RobustKernel> robustKernel_;
-    int64_t internalId_;
+    int64_t internalId_ = -1;
 
     void resizeParameters(size_t newSize) {
       parameters_.resize(newSize, nullptr);

diff --git a/g2o/core/optimization_algorithm_dogleg.cpp b/g2o/core/optimization_algorithm_dogleg.cpp
@@ -47,19 +47,17 @@ namespace g2o {
 
 OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(
     std::unique_ptr<BlockSolverBase> solver)
-    : OptimizationAlgorithmWithHessian(*solver), m_solver_{std::move(solver)} {
-  userDeltaInit_ = properties_.makeProperty<Property<double>>(
-      "initialDelta", static_cast<double>(1e4));
-  maxTrialsAfterFailure_ =
-      properties_.makeProperty<Property<int>>("maxTrialsAfterFailure", 100);
-  initialLambda_ = properties_.makeProperty<Property<double>>(
-      "initialLambda", static_cast<double>(1e-7));
-  lamdbaFactor_ =
-      properties_.makeProperty<Property<double>>("lambdaFactor", 10.);
-  delta_ = userDeltaInit_->value();
-}
-
-OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg() = default;
+    : OptimizationAlgorithmWithHessian(*solver),
+      maxTrialsAfterFailure_(properties_.makeProperty<Property<int>>(
+          "maxTrialsAfterFailure", 100)),
+      userDeltaInit_(properties_.makeProperty<Property<double>>(
+          "initialDelta", static_cast<double>(1e4))),
+      initialLambda_(properties_.makeProperty<Property<double>>(
+          "initialLambda", static_cast<double>(1e-7))),
+      lamdbaFactor_(
+          properties_.makeProperty<Property<double>>("lambdaFactor", 10.)),
+      delta_(userDeltaInit_->value()),
+      m_solver_{std::move(solver)} {}
 
 OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(
     int iteration, bool online) {

diff --git a/g2o/core/optimization_algorithm_dogleg.h b/g2o/core/optimization_algorithm_dogleg.h
@@ -54,7 +54,7 @@ class G2O_CORE_API OptimizationAlgorithmDogleg
    * the linearized system.
    */
   explicit OptimizationAlgorithmDogleg(std::unique_ptr<BlockSolverBase> solver);
-  ~OptimizationAlgorithmDogleg() override;
+  ~OptimizationAlgorithmDogleg() = default;
 
   SolverResult solve(int iteration, bool online = false) override;
 

diff --git a/g2o/core/optimization_algorithm_levenberg.cpp b/g2o/core/optimization_algorithm_levenberg.cpp
@@ -50,12 +50,12 @@ namespace g2o {
 
 OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(
     std::unique_ptr<Solver> solver)
-    : OptimizationAlgorithmWithHessian(*solver), m_solver_{std::move(solver)} {
-  userLambdaInit_ =
-      properties_.makeProperty<Property<double> >("initialLambda", 0.);
-  maxTrialsAfterFailure_ =
-      properties_.makeProperty<Property<int> >("maxTrialsAfterFailure", 10);
-}
+    : OptimizationAlgorithmWithHessian(*solver),
+      maxTrialsAfterFailure_(properties_.makeProperty<Property<int> >(
+          "maxTrialsAfterFailure", 10)),
+      userLambdaInit_(
+          properties_.makeProperty<Property<double> >("initialLambda", 0.)),
+      m_solver_{std::move(solver)} {}
 
 OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(
     int iteration, bool online) {

diff --git a/g2o/examples/convert_segment_line/convertSegmentLine.cpp b/g2o/examples/convert_segment_line/convertSegmentLine.cpp
@@ -62,8 +62,8 @@ Eigen::Vector2d computeLine(const Eigen::Vector2d& p1,
 }
 
 struct LineInfo {
-  explicit LineInfo(VertexSegment2D* s) {
-    line = std::make_shared<VertexLine2D>();
+  explicit LineInfo(VertexSegment2D* s)
+      : line(std::make_shared<VertexLine2D>()) {
     line->setId(s->id());
     line->setEstimate(Line2D(computeLine(s->estimateP1(), s->estimateP2())));
   }

diff --git a/g2o/simulator/sensor_line3d.cpp b/g2o/simulator/sensor_line3d.cpp
@@ -29,73 +29,63 @@
 #include <cassert>
 
 namespace g2o {
-using namespace std;
 
 // SensorLine3D
 SensorLine3D::SensorLine3D(const std::string& name_)
     : BinarySensor<Robot3D, EdgeSE3Line3D, WorldObjectLine3D>(name_) {
-  _offsetParam = 0;
-  _information.setIdentity();
-  _information *= 1e9;
-  //_information(2,2)=10;
-  setInformation(_information);
+  information_.setIdentity();
+  information_ *= 1e9;
+  // information_(2,2)=10;
+  setInformation(information_);
 }
 
-bool SensorLine3D::isVisible(SensorLine3D::WorldObjectType* to) {
-  if (!_robotPoseObject) return false;
+bool SensorLine3D::isVisible(SensorLine3D::WorldObjectType* /*to*/) {
+  return false;
+  /* TODO(Rainer): implement
+  if (!robotPoseVertex_) return false;
   assert(to && to->vertex());
   VertexType::EstimateType pose = to->vertex()->estimate();
-  VertexType::EstimateType delta = _sensorPose.inverse() * pose;
+  VertexType::EstimateType delta = sensorPose_.inverse() * pose;
   Vector3 translation = delta.translation();
   double range2 = translation.squaredNorm();
   if (range2 > _maxRange2) return false;
   if (range2 < _minRange2) return false;
   translation.normalize();
   // the cameras have the z in front
   double bearing = acos(translation.z());
-  if (fabs(bearing) > _fov) return false;
-  return true;
+  return fabs(bearing) <= _fov;
+  */
 }
 
-void SensorLine3D::addParameters() {
-  if (!_offsetParam) _offsetParam = std::make_shared<ParameterSE3Offset>();
-  assert(world());
-  world()->addParameter(_offsetParam);
+void SensorLine3D::addParameters(World& world) {
+  if (!offsetParam_) offsetParam_ = std::make_shared<ParameterSE3Offset>();
+  world.addParameter(offsetParam_);
 }
 
-void SensorLine3D::addNoise(EdgeType* e) {
-  EdgeType::ErrorVector n = _sampler.generateSample();
+void SensorLine3D::addNoise(EdgeType* /*e*/) {
+  /* TODO(Rainer): implement
+  EdgeType::ErrorVector n = sampler_.generateSample();
   e->setMeasurement(e->measurement() + n);
   e->setInformation(information());
+  */
 }
 
-void SensorLine3D::sense() {
-  if (!_offsetParam) {
+void SensorLine3D::sense(BaseRobot& robot, World& world) {
+  if (!offsetParam_) {
     return;
   }
-  _robotPoseObject = 0;
-  RobotType* r = dynamic_cast<RobotType*>(robot());
-  std::list<PoseObject*>::reverse_iterator it = r->trajectory().rbegin();
-  int count = 0;
-  while (it != r->trajectory().rend() && count < 1) {
-    if (!_robotPoseObject) _robotPoseObject = *it;
-    ++it;
-    count++;
-  }
-  if (!_robotPoseObject) return;
-  _sensorPose = _robotPoseObject->vertex()->estimate() * _offsetParam->offset();
-  for (std::set<BaseWorldObject*>::iterator it = world()->objects().begin();
-       it != world()->objects().end(); ++it) {
-    WorldObjectType* o = dynamic_cast<WorldObjectType*>(*it);
-    if (o && isVisible(o)) {
-      auto e = mkEdge(o);
-      if (e && graph()) {
-        e->setParameterId(0, _offsetParam->id());
-        graph()->addEdge(e);
-        e->setMeasurementFromState();
-        addNoise(e.get());
-      }
-    }
+  robotPoseVertex_ = robotPoseVertex<PoseVertexType>(robot, world);
+  if (!robotPoseVertex_) return;
+  sensorPose_ = robotPoseVertex_->estimate() * offsetParam_->param();
+  for (const auto& it : world.objects()) {
+    auto* o = dynamic_cast<WorldObjectType*>(it.get());
+    if (!o || !isVisible(o)) continue;
+    auto e = mkEdge(o);
+    if (!e) continue;
+    e->setParameterId(0, offsetParam_->id());
+    world.graph().addEdge(e);
+    e->setMeasurementFromState();
+    addNoise(e.get());
   }
 }
 

diff --git a/g2o/simulator/sensor_line3d.h b/g2o/simulator/sensor_line3d.h
@@ -26,6 +26,8 @@
 
 #ifndef G2O_SENSOR_LINE3D_H_
 #define G2O_SENSOR_LINE3D_H_
+#include "g2o/types/slam3d/parameter_se3_offset.h"
+#include "g2o/types/slam3d_addons/edge_se3_line.h"
 #include "g2o_simulator_api.h"
 #include "pointsensorparameters.h"
 #include "simulator3d_base.h"
@@ -36,17 +38,18 @@ class G2O_SIMULATOR_API SensorLine3D
     : public PointSensorParameters,
       public BinarySensor<Robot3D, EdgeSE3Line3D, WorldObjectLine3D> {
  public:
-  typedef PoseVertexType::EstimateType RobotPoseType;
+  using RobotPoseType = PoseVertexType::EstimateType;
   explicit SensorLine3D(const std::string& name_);
-  virtual void sense();
-  virtual void addParameters();
-  std::shared_ptr<ParameterSE3Offset> offsetParam() { return _offsetParam; };
-  void addNoise(EdgeType* e);
+  void sense(BaseRobot& robot, World& world) override;
+  void addNoise(EdgeType* e) override;
+  std::shared_ptr<ParameterSE3Offset> offsetParam() { return offsetParam_; };
+
+  void addParameters(World& world) override;
 
  protected:
   bool isVisible(WorldObjectType* to);
-  RobotPoseType _sensorPose;
-  std::shared_ptr<ParameterSE3Offset> _offsetParam;
+  RobotPoseType sensorPose_;
+  std::shared_ptr<ParameterSE3Offset> offsetParam_;
 };
 
 }  // namespace g2o

diff --git a/g2o/simulator/sensor_pointxyz.cpp b/g2o/simulator/sensor_pointxyz.cpp
@@ -37,7 +37,6 @@ namespace g2o {
 SensorPointXYZ::SensorPointXYZ(std::string name)
     : BinarySensor<Robot3D, EdgeSE3PointXYZ, WorldObjectTrackXYZ>(
           std::move(name)) {
-  offsetParam_ = nullptr;
   information_.setIdentity();
   information_ *= 1000;
   information_(2, 2) = 10;

diff --git a/g2o/simulator/sensor_pointxyz_disparity.cpp b/g2o/simulator/sensor_pointxyz_disparity.cpp
@@ -36,7 +36,6 @@ namespace g2o {
 SensorPointXYZDisparity::SensorPointXYZDisparity(const std::string& name)
     : BinarySensor<Robot3D, EdgeSE3PointXYZDisparity, WorldObjectTrackXYZ>(
           name) {
-  offsetParam_ = nullptr;
   information_.setIdentity();
   information_ *= 1000;
   setInformation(information_);

diff --git a/g2o/simulator/sensor_pose3d_offset.cpp b/g2o/simulator/sensor_pose3d_offset.cpp
@@ -37,8 +37,6 @@ namespace g2o {
 
 SensorPose3DOffset::SensorPose3DOffset(std::string name)
     : BinarySensor<Robot3D, EdgeSE3Offset, WorldObjectSE3>(std::move(name)) {
-  offsetParam1_ = offsetParam2_ = nullptr;
-  stepsToIgnore_ = 10;
   information_.setIdentity();
   information_ *= 100;
   information_(3, 3) = 10000;

diff --git a/g2o/simulator/sensor_pose3d_offset.h b/g2o/simulator/sensor_pose3d_offset.h
@@ -50,7 +50,7 @@ class SensorPose3DOffset
 
  protected:
   bool isVisible(WorldObjectType* to);
-  int stepsToIgnore_;
+  int stepsToIgnore_ = 10;
   std::shared_ptr<ParameterSE3Offset> offsetParam1_, offsetParam2_;
 };
 

diff --git a/g2o/types/icp/types_icp.cpp b/g2o/types/icp/types_icp.cpp
@@ -156,10 +156,7 @@ void Edge_XYZ_VSC::linearizeOplus() {
   double py = pc(1);
   double pz = pc(2);
   double ipz2 = 1.0 / (pz * pz);
-  if (std::isnan(ipz2)) {
-    std::cout << "[SetJac] infinite jac" << std::endl;
-    *(int*)0x0 = 0;
-  }
+  assert(!std::isnan(ipz2) && "[SetJac] infinite jac");
 
   double ipz2fx = ipz2 * vc->Kcam(0, 0);  // Fx
   double ipz2fy = ipz2 * vc->Kcam(1, 1);  // Fy

diff --git a/g2o/types/slam3d_addons/line3d.cpp b/g2o/types/slam3d_addons/line3d.cpp
@@ -43,7 +43,7 @@ inline Matrix3 skew(const Vector3& t) {
 
 Line3D::Line3D() { line << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0; }
 
-Line3D::Line3D(const Vector6& v) { line = v; }
+Line3D::Line3D(const Vector6& v) : line(v) {}  // NOLINT
 
 void Line3D::setW(const Vector3& w_) { line.head<3>() = w_; }
 

diff --git a/python/examples/ba_demo.py b/python/examples/ba_demo.py
@@ -1,7 +1,6 @@
 # https://github.com/RainerKuemmerle/g2o/blob/master/g2o/examples/ba/ba_demo.cpp
 
 import argparse
-from collections import defaultdict
 
 import g2opy as g2o
 import numpy as np

diff --git a/python/examples/gicp_demo.py b/python/examples/gicp_demo.py
@@ -40,16 +40,16 @@ def main():
     trans1 = optimizer.vertex(1).estimate().inverse()
 
     # set up point matches
-    for i in range(len(true_points)):
+    for i, pt in enumerate(true_points):
         # calculate the relative 3d position of the point
-        pt0 = trans0 * true_points[i]
-        pt1 = trans1 * true_points[i]
+        pt0 = trans0 * pt
+        pt1 = trans1 * pt
 
         # add noise
         pt0 += np.random.randn(3) * args.noise
         pt1 += np.random.randn(3) * args.noise
 
-        # form edge, with normals in varioius positions
+        # form edge, with normals in various positions
         nm0 = np.array([0, i, 1])
         nm0 = nm0 / np.linalg.norm(nm0)
         nm1 = np.array([0, i, 1])