diff --git a/g2o/core/sparse_block_matrix.hpp b/g2o/core/sparse_block_matrix.hpp index d24bcae96..0de0a3d0d 100644 --- a/g2o/core/sparse_block_matrix.hpp +++ b/g2o/core/sparse_block_matrix.hpp @@ -26,6 +26,8 @@ #include +#include "g2o/core/eigen_types.h" + namespace g2o { template @@ -364,15 +366,15 @@ void SparseBlockMatrix::scale(double a_) { template SparseBlockMatrix* SparseBlockMatrix::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); diff --git a/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.cpp b/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.cpp index b1b492514..1d64941ea 100644 --- a/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.cpp +++ b/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.cpp @@ -46,9 +46,9 @@ std::unique_ptr 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; } @@ -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 backupIdx(_touchedVertices.size()); int idx = 0; for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin(); it != _touchedVertices.end(); ++it) { @@ -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); @@ -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); diff --git a/g2o/solvers/structure_only/structure_only_solver.h b/g2o/solvers/structure_only/structure_only_solver.h index 0f5a40b3f..e99a9b5b3 100644 --- a/g2o/solvers/structure_only/structure_only_solver.h +++ b/g2o/solvers/structure_only/structure_only_solver.h @@ -118,11 +118,7 @@ class StructureOnlySolver : public OptimizationAlgorithm { dynamic_cast(*it_t); // fix all the other vertices and remember their fix value -#ifdef WINDOWS std::vector 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(e->vertex(k)); diff --git a/g2o/types/slam2d_addons/edge_line2d.cpp b/g2o/types/slam2d_addons/edge_line2d.cpp index ebfe69a24..84348bb22 100644 --- a/g2o/types/slam2d_addons/edge_line2d.cpp +++ b/g2o/types/slam2d_addons/edge_line2d.cpp @@ -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>() { @@ -56,3 +50,5 @@ void EdgeLine2D::linearizeOplus() { _jacobianOplusXj = Matrix2::Identity(); } #endif + +} // namespace g2o diff --git a/g2o/types/slam2d_addons/edge_line2d.h b/g2o/types/slam2d_addons/edge_line2d.h index 80374b714..f824aa6aa 100644 --- a/g2o/types/slam2d_addons/edge_line2d.h +++ b/g2o/types/slam2d_addons/edge_line2d.h @@ -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 { diff --git a/g2o/types/slam2d_addons/edge_line2d_pointxy.h b/g2o/types/slam2d_addons/edge_line2d_pointxy.h index ff9a9da98..341f17d4b 100644 --- a/g2o/types/slam2d_addons/edge_line2d_pointxy.h +++ b/g2o/types/slam2d_addons/edge_line2d_pointxy.h @@ -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(_vertices[0]); diff --git a/g2o/types/slam2d_addons/edge_se2_line2d.h b/g2o/types/slam2d_addons/edge_se2_line2d.h index 0895ea68b..24217dcbf 100644 --- a/g2o/types/slam2d_addons/edge_se2_line2d.h +++ b/g2o/types/slam2d_addons/edge_se2_line2d.h @@ -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" diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d.h b/g2o/types/slam2d_addons/edge_se2_segment2d.h index 13b9e3ab1..6587ce540 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d.h @@ -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(&(_measurement[0])); diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h index 5448706a6..aac6122c6 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h @@ -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]; } @@ -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 diff --git a/g2o/types/slam2d_addons/vertex_line2d.cpp b/g2o/types/slam2d_addons/vertex_line2d.cpp index 5804587b2..7a20f6edf 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.cpp +++ b/g2o/types/slam2d_addons/vertex_line2d.cpp @@ -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 diff --git a/g2o/types/slam2d_addons/vertex_line2d.h b/g2o/types/slam2d_addons/vertex_line2d.h index f438b6efd..894881867 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.h +++ b/g2o/types/slam2d_addons/vertex_line2d.h @@ -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" diff --git a/g2o/types/slam3d_addons/edge_se3_calib.h b/g2o/types/slam3d_addons/edge_se3_calib.h index a1c6bc2ff..0d9723ed4 100644 --- a/g2o/types/slam3d_addons/edge_se3_calib.h +++ b/g2o/types/slam3d_addons/edge_se3_calib.h @@ -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);