error in optimizer.addEdge

[ycchen810720]

Here is the short sample code I have tried to see if library works or not.
program always died in optimizer.addEdge() -> e->resolveParameters() -> if (typeid(aux).name() != _parameterTypes[i])
It seems like aux is 0x0
I tried old version g2o (2020)(2023), the same result.

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <Eigen/Core>
#include

int main()
{
g2o::SparseOptimizer optimizer;
auto linearSolver = std::make_unique<g2o::LinearSolverDenseg2o::BlockSolverX::PoseMatrixType>();
auto blockSolver = std::make_uniqueg2o::BlockSolverX(std::move(linearSolver));
auto algorithm = new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver));
optimizer.setAlgorithm(algorithm);

g2o::VertexSE3 *poseVertex = new g2o::VertexSE3();
poseVertex->setId(0);
poseVertex->setEstimate(Eigen::Isometry3d::Identity());
optimizer.addVertex(poseVertex);

g2o::VertexPointXYZ *pointVertex = new g2o::VertexPointXYZ();
pointVertex->setId(1);
pointVertex->setEstimate(Eigen::Vector3d(1.0, 1.0, 5.0)); 
optimizer.addVertex(pointVertex);

g2o::EdgeSE3PointXYZ *edge = new g2o::EdgeSE3PointXYZ();
edge->setVertex(0, poseVertex);                      
edge->setVertex(1, pointVertex);                      
edge->setMeasurement(Eigen::Vector3d(1.0, 1.0, 5.0));
edge->setInformation(Eigen::Matrix3d::Identity());   
std::cout << "Vertex 0 type: " << typeid(*optimizer.vertex(0)).name() << std::endl;//Vertex 0 type: N3g2o9VertexSE3E
std::cout << "Vertex 1 type: " << typeid(*optimizer.vertex(1)).name() << std::endl;//ertex 1 type: N3g2o14VertexPointXYZE
std::cout << "Vertex 0 exists: " << (optimizer.vertex(0) != nullptr) << std::endl;//Vertex 0 exists: 1
std::cout << "Vertex 1 exists: " << (optimizer.vertex(1) != nullptr) << std::endl;//Vertex 1 exists: 1
std::cout << "Measurement: " << edge->measurement().transpose() << std::endl;//Measurement: 1 1 5
std::cout << "Information matrix:\n"
          << edge->information() << std::endl;//Information matrix: 1 0 0 0 1 0 0 0 1
optimizer.addEdge(edge);//error
return 0;

}

[RainerKuemmerle]

You also need to add a parameter for modelling the position of the camera relative to the pose of the vertex.

I see that the error handling here is broken and it actually generates a segmentation fault. I will investigate this.

You can fix your example. Relevant additions would be

  g2o::ParameterSE3Offset* paramOffset = new g2o::ParameterSE3Offset;
  paramOffset->setId(0);
  optimizer.addParameter(paramOffset);

  g2o::EdgeSE3PointXYZ* edge = new g2o::EdgeSE3PointXYZ();
  edge->setVertex(0, poseVertex);
  edge->setVertex(1, pointVertex);
  edge->setMeasurement(Eigen::Vector3d(1.0, 1.0, 5.0));
  edge->setInformation(Eigen::Matrix3d::Identity());
  edge->setParameterId(0, paramOffset->id());

Full example

#include <iostream>

#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/types/slam3d/edge_se3_pointxyz.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/config.h"

#if defined G2O_HAVE_CHOLMOD
G2O_USE_OPTIMIZATION_LIBRARY(cholmod);
#else
G2O_USE_OPTIMIZATION_LIBRARY(eigen);
#endif

int main() {
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);
  std::string solverName = "lm_fix6_3";
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(
      g2o::OptimizationAlgorithmFactory::instance()->construct(solverName,
                                                               solverProperty));

  g2o::VertexSE3* poseVertex = new g2o::VertexSE3();
  poseVertex->setId(0);
  poseVertex->setEstimate(Eigen::Isometry3d::Identity());
  optimizer.addVertex(poseVertex);

  g2o::VertexPointXYZ* pointVertex = new g2o::VertexPointXYZ();
  pointVertex->setId(1);
  pointVertex->setEstimate(Eigen::Vector3d(1.0, 1.0, 5.0));
  optimizer.addVertex(pointVertex);

  g2o::ParameterSE3Offset* paramOffset = new g2o::ParameterSE3Offset;
  paramOffset->setId(0);
  optimizer.addParameter(paramOffset);

  g2o::EdgeSE3PointXYZ* edge = new g2o::EdgeSE3PointXYZ();
  edge->setVertex(0, poseVertex);
  edge->setVertex(1, pointVertex);
  edge->setMeasurement(Eigen::Vector3d(1.0, 1.0, 5.0));
  edge->setInformation(Eigen::Matrix3d::Identity());
  edge->setParameterId(0, paramOffset->id());

  std::cout << "Vertex 0 type: " << typeid(*optimizer.vertex(0)).name()
            << std::endl;  // Vertex 0 type: N3g2o9VertexSE3E
  std::cout << "Vertex 1 type: " << typeid(*optimizer.vertex(1)).name()
            << std::endl;  // ertex 1 type: N3g2o14VertexPointXYZE
  std::cout << "Vertex 0 exists: " << (optimizer.vertex(0) != nullptr)
            << std::endl;  // Vertex 0 exists: 1
  std::cout << "Vertex 1 exists: " << (optimizer.vertex(1) != nullptr)
            << std::endl;  // Vertex 1 exists: 1
  std::cout << "Measurement: " << edge->measurement().transpose()
            << std::endl;  // Measurement: 1 1 5
  std::cout << "Information matrix:\n"
            << edge->information()
            << std::endl;  // Information matrix: 1 0 0 0 1 0 0 0 1
  bool status = optimizer.addEdge(edge);  // error
  std::cout << status << std::endl;
  return 0;
}

[github-actions]

This issue is stale because it has been open 60 days with no activity. Remove stale label or comment or this will be closed in 14 days.

[github-actions]

This issue was closed because it has been stalled for 14 days with no activity.