Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions g2o/core/sparse_block_matrix.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@

#include <cassert>

#include "g2o/core/eigen_types.h"

namespace g2o {

template <class MatrixType>
Expand Down Expand Up @@ -364,15 +366,15 @@ void SparseBlockMatrix<MatrixType>::scale(double a_) {
template <class MatrixType>
SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::slice(
int rmin, int rmax, int cmin, int cmax, bool alloc) const {
int m = rmax - rmin;
int n = cmax - cmin;
int rowIdx[m];
const int m = rmax - rmin;
const int n = cmax - cmin;
VectorX rowIdx(m);
rowIdx[0] = rowsOfBlock(rmin);
for (int i = 1; i < m; ++i) {
rowIdx[i] = rowIdx[i - 1] + rowsOfBlock(rmin + i);
}

int colIdx[n];
VectorX colIdx(n);
colIdx[0] = colsOfBlock(cmin);
for (int i = 1; i < n; ++i) {
colIdx[i] = colIdx[i - 1] + colsOfBlock(cmin + i);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,9 @@ std::unique_ptr<g2o::Solver> AllocateCholmodSolver() {
* \brief backing up some information about the vertex
*/
struct VertexBackup {
int hessianIndex;
OptimizableGraph::Vertex* vertex;
double* hessianData;
int hessianIndex = 0;
OptimizableGraph::Vertex* vertex = nullptr;
double* hessianData = nullptr;
bool operator<(const VertexBackup& other) const {
return hessianIndex < other.hessianIndex;
}
Expand Down Expand Up @@ -235,12 +235,7 @@ bool SparseOptimizerIncremental::updateInitialization(
// cerr << "updating index mapping done." << endl;

// backup the tempindex and prepare sorting structure
#ifdef _MSC_VER
VertexBackup* backupIdx = new VertexBackup[_touchedVertices.size()];
#else
VertexBackup backupIdx[_touchedVertices.size()];
#endif
memset(backupIdx, 0, sizeof(VertexBackup) * _touchedVertices.size());
std::vector<VertexBackup> backupIdx(_touchedVertices.size());
int idx = 0;
for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin();
it != _touchedVertices.end(); ++it) {
Expand All @@ -250,10 +245,8 @@ bool SparseOptimizerIncremental::updateInitialization(
backupIdx[idx].hessianData = v->hessianData();
++idx;
}
sort(backupIdx,
backupIdx +
_touchedVertices
.size()); // sort according to the hessianIndex which is the
sort(backupIdx.begin(),
backupIdx.end()); // sort according to the hessianIndex which is the
// same order as used later by the optimizer
for (int i = 0; i < idx; ++i) {
backupIdx[i].vertex->setHessianIndex(i);
Expand Down Expand Up @@ -380,9 +373,6 @@ bool SparseOptimizerIncremental::updateInitialization(
}
}
cholmod_free_sparse(&updateAsSparseFactor, &_cholmodCommon);
#ifdef _MSC_VER
delete[] backupIdx;
#endif

#if 0
cholmod_sparse* updatePermuted = cholmod_triplet_to_sparse(_permutedUpdate, _permutedUpdate->nnz, &_cholmodCommon);
Expand Down
4 changes: 0 additions & 4 deletions g2o/solvers/structure_only/structure_only_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -118,11 +118,7 @@ class StructureOnlySolver : public OptimizationAlgorithm {
dynamic_cast<g2o::OptimizableGraph::Edge*>(*it_t);

// fix all the other vertices and remember their fix value
#ifdef WINDOWS
std::vector<bool> remember_fix_status(e->vertices().size());
#else
bool remember_fix_status[e->vertices().size()];
#endif
for (size_t k = 0; k < e->vertices().size(); ++k) {
OptimizableGraph::Vertex* otherV =
static_cast<OptimizableGraph::Vertex*>(e->vertex(k));
Expand Down
10 changes: 3 additions & 7 deletions g2o/types/slam2d_addons/edge_line2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,7 @@

#include "edge_line2d.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

using namespace g2o;
using namespace Eigen;
namespace g2o {

EdgeLine2D::EdgeLine2D()
: BaseBinaryEdge<2, Line2D, VertexLine2D, VertexLine2D>() {
Expand All @@ -56,3 +50,5 @@ void EdgeLine2D::linearizeOplus() {
_jacobianOplusXj = Matrix2::Identity();
}
#endif

} // namespace g2o
3 changes: 1 addition & 2 deletions g2o/types/slam2d_addons/edge_line2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,8 @@
#ifndef G2O_EDGE_LINE2D_H
#define G2O_EDGE_LINE2D_H

#include "g2o/config.h"
#include "g2o/core/base_binary_edge.h"
#include "types_slam2d_addons.h"
#include "g2o_types_slam2d_addons_api.h"
#include "vertex_line2d.h"

namespace g2o {
Expand Down
9 changes: 2 additions & 7 deletions g2o/types/slam2d_addons/edge_line2d_pointxy.h
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,9 @@
namespace g2o {

class EdgeLine2DPointXY
: public BaseBinaryEdge<1, double, VertexLine2D,
VertexPointXY> // Avoid redefinition of BaseEdge in
// MSVC
{
: public BaseBinaryEdge<1, double, VertexLine2D, VertexPointXY> {
public:
G2O_TYPES_SLAM2D_ADDONS_API EIGEN_MAKE_ALIGNED_OPERATOR_NEW
G2O_TYPES_SLAM2D_ADDONS_API
EdgeLine2DPointXY();
G2O_TYPES_SLAM2D_ADDONS_API EdgeLine2DPointXY();

G2O_TYPES_SLAM2D_ADDONS_API void computeError() {
const VertexLine2D* l = static_cast<const VertexLine2D*>(_vertices[0]);
Expand Down
1 change: 0 additions & 1 deletion g2o/types/slam2d_addons/edge_se2_line2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
#ifndef G2O_EDGE_SE2_LINE2D_H
#define G2O_EDGE_SE2_LINE2D_H

#include "g2o/config.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/stuff/misc.h"
#include "g2o/types/slam2d/vertex_se2.h"
Expand Down
9 changes: 2 additions & 7 deletions g2o/types/slam2d_addons/edge_se2_segment2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,9 @@
namespace g2o {

class EdgeSE2Segment2D
: public BaseBinaryEdge<4, Vector4, VertexSE2,
VertexSegment2D> // Avoid redefinition of BaseEdge
// in MSVC
{
: public BaseBinaryEdge<4, Vector4, VertexSE2, VertexSegment2D> {
public:
G2O_TYPES_SLAM2D_ADDONS_API EIGEN_MAKE_ALIGNED_OPERATOR_NEW
G2O_TYPES_SLAM2D_ADDONS_API
EdgeSE2Segment2D();
G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2D();

G2O_TYPES_SLAM2D_ADDONS_API Vector2 measurementP1() {
return Eigen::Map<const Vector2>(&(_measurement[0]));
Expand Down
33 changes: 2 additions & 31 deletions g2o/types/slam2d_addons/edge_se2_segment2d_line.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,9 @@
namespace g2o {

class EdgeSE2Segment2DLine
: public BaseBinaryEdge<2, Vector2, VertexSE2,
VertexSegment2D> // Avoid redefinition of BaseEdge
// in MSVC
{
: public BaseBinaryEdge<2, Vector2, VertexSE2, VertexSegment2D> {
public:
G2O_TYPES_SLAM2D_ADDONS_API EIGEN_MAKE_ALIGNED_OPERATOR_NEW
G2O_TYPES_SLAM2D_ADDONS_API
EdgeSE2Segment2DLine();
G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLine();

G2O_TYPES_SLAM2D_ADDONS_API double theta() const { return _measurement[0]; }
G2O_TYPES_SLAM2D_ADDONS_API double rho() const { return _measurement[1]; }
Expand Down Expand Up @@ -102,32 +97,8 @@ class EdgeSE2Segment2DLine

G2O_TYPES_SLAM2D_ADDONS_API virtual bool read(std::istream& is);
G2O_TYPES_SLAM2D_ADDONS_API virtual bool write(std::ostream& os) const;

/* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */
/* virtual void linearizeOplus(); */
/* #endif */
};

/* class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLineWriteGnuplotAction:
* public WriteGnuplotAction { */
/* public: */
/* EdgeSE2Segment2DLineWriteGnuplotAction(); */
/* virtual HyperGraphElementAction*
* operator()(HyperGraph::HyperGraphElement* element, */
/* HyperGraphElementAction::Parameters* params_); */
/* }; */

/* #ifdef G2O_HAVE_OPENGL */
/* class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLineDrawAction: public
* DrawAction{ */
/* public: */
/* EdgeSE2Segment2DLineDrawAction(); */
/* virtual HyperGraphElementAction*
* operator()(HyperGraph::HyperGraphElement* element, */
/* HyperGraphElementAction::Parameters* params_); */
/* }; */
/* #endif */

} // namespace g2o

#endif
2 changes: 2 additions & 0 deletions g2o/types/slam2d_addons/vertex_line2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@

#include "vertex_line2d.h"

#include "g2o/types/slam2d/vertex_point_xy.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_wrapper.h"
#endif
Expand Down
1 change: 0 additions & 1 deletion g2o/types/slam2d_addons/vertex_line2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"
#include "g2o/stuff/misc.h"
#include "g2o/types/slam2d/vertex_point_xy.h"
#include "g2o_types_slam2d_addons_api.h"
#include "line_2d.h"

Expand Down
4 changes: 1 addition & 3 deletions g2o/types/slam3d_addons/edge_se3_calib.h
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,7 @@ class EdgeSE3Calib
// BaseEdge in MSVC
{
public:
G2O_TYPES_SLAM3D_ADDONS_API EIGEN_MAKE_ALIGNED_OPERATOR_NEW
G2O_TYPES_SLAM3D_ADDONS_API
EdgeSE3Calib();
G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3Calib();

G2O_TYPES_SLAM3D_ADDONS_API void computeError();
G2O_TYPES_SLAM3D_ADDONS_API virtual bool read(std::istream& is);
Expand Down