From 053069912386a5c3a4fb0757b43e29e832cd39a4 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 11 May 2014 12:31:45 +0200 Subject: [PATCH] syntax cleanups so far - preparing merging into master - there are large commented code blocks - need to do something about the namespaces mess --- CMakeLists.txt | 10 +- Makefile | 2 +- g2o/apps/g2o_hierarchical/CMakeLists.txt | 2 +- g2o/apps/g2o_hierarchical/backbone_tree_action.cpp | 14 +- g2o/apps/g2o_hierarchical/backbone_tree_action.h | 61 +-- g2o/apps/g2o_hierarchical/edge_creator.h | 13 +- g2o/apps/g2o_hierarchical/edge_labeler.cpp | 18 +- g2o/apps/g2o_hierarchical/edge_labeler.h | 18 +- .../g2o_hierarchical/edge_types_cost_function.cpp | 8 +- .../g2o_hierarchical/edge_types_cost_function.h | 8 +- g2o/apps/g2o_hierarchical/g2o_hierarchical.cpp | 55 ++- .../g2o_hierarchical_test_functions.cpp | 82 ++-- g2o/apps/g2o_hierarchical/simple_star_ops.cpp | 165 ++++----- g2o/apps/g2o_hierarchical/simple_star_ops.h | 15 +- g2o/apps/g2o_hierarchical/star.cpp | 4 +- g2o/apps/g2o_hierarchical/star.h | 29 +- g2o/apps/g2o_simulator/convertSegmentLine.cpp | 10 +- .../g2o_simulator/g2o_anonymize_observations.cpp | 4 +- g2o/apps/g2o_simulator/sensor_pose3d.h | 4 +- g2o/apps/g2o_simulator/sensor_segment2d.cpp | 60 +-- g2o/apps/g2o_simulator/sensor_segment2d.h | 6 +- g2o/apps/g2o_simulator/sensor_segment2d_line.cpp | 60 +-- g2o/apps/g2o_simulator/sensor_segment2d_line.h | 6 +- .../g2o_simulator/sensor_segment2d_pointline.cpp | 60 +-- .../g2o_simulator/sensor_segment2d_pointline.h | 6 +- g2o/apps/g2o_simulator/simulator.h | 411 +++++++++++---------- g2o/apps/g2o_simulator/simulator2d_segment.cpp | 17 +- g2o/apps/g2o_simulator/test_simulator2d.cpp | 15 +- g2o/core/base_edge.h | 12 +- g2o/core/base_multi_edge.h | 28 +- g2o/core/base_multi_edge.hpp | 6 +- g2o/core/base_unary_edge.hpp | 3 +- g2o/core/hyper_graph.cpp | 29 +- g2o/core/hyper_graph.h | 59 ++- g2o/core/optimizable_graph.cpp | 43 +-- g2o/core/optimizable_graph.h | 29 +- g2o/examples/plane_slam/plane_test.cpp | 11 +- g2o/examples/plane_slam/simulator_3d_plane.cpp | 65 ++-- g2o/stuff/unscented.h | 2 +- g2o/types/data/vertex_ellipse.cpp | 13 +- g2o/types/data/vertex_ellipse.h | 16 +- g2o/types/slam2d/edge_se2.cpp | 2 +- g2o/types/slam2d/edge_se2_lotsofxy.cpp | 115 +++--- g2o/types/slam2d/edge_se2_lotsofxy.h | 68 ++-- g2o/types/slam2d/edge_se2_twopointsxy.cpp | 56 +-- g2o/types/slam2d/edge_se2_twopointsxy.h | 30 +- g2o/types/slam2d/se2.h | 14 +- g2o/types/slam2d/types_slam2d.cpp | 2 +- g2o/types/slam2d/vertex_point_xy.cpp | 3 +- g2o/types/slam2d/vertex_point_xy.h | 24 +- g2o/types/slam2d/vertex_se2.cpp | 6 +- g2o/types/slam2d_addons/CMakeLists.txt | 1 - g2o/types/slam2d_addons/edge_line2d.h | 2 +- g2o/types/slam2d_addons/edge_line2d_pointxy.cpp | 4 +- g2o/types/slam2d_addons/edge_line2d_pointxy.h | 4 +- g2o/types/slam2d_addons/edge_se2_line2d.cpp | 4 +- g2o/types/slam2d_addons/edge_se2_line2d.h | 2 +- g2o/types/slam2d_addons/edge_se2_segment2d.cpp | 4 +- g2o/types/slam2d_addons/edge_se2_segment2d.h | 4 +- g2o/types/slam2d_addons/edge_se2_segment2d_line.h | 2 +- .../slam2d_addons/edge_se2_segment2d_pointLine.h | 4 +- g2o/types/slam2d_addons/types_slam2d_addons.cpp | 6 +- g2o/types/slam2d_addons/vertex_line2d.cpp | 8 +- g2o/types/slam2d_addons/vertex_line2d.h | 15 +- g2o/types/slam2d_addons/vertex_segment2d.cpp | 10 +- g2o/types/slam2d_addons/vertex_segment2d.h | 11 +- g2o/types/slam3d/edge_se3_lotsofxyz.cpp | 152 ++++---- g2o/types/slam3d/edge_se3_lotsofxyz.h | 67 ++-- g2o/types/slam3d/edge_se3_offset.cpp | 8 +- g2o/types/slam3d/types_slam3d.cpp | 10 +- g2o/types/slam3d_addons/CMakeLists.txt | 4 +- g2o/types/slam3d_addons/edge_se3_calib.cpp | 4 +- g2o/types/slam3d_addons/edge_se3_euler.cpp | 11 +- g2o/types/slam3d_addons/edge_se3_euler.h | 12 +- g2o/types/slam3d_addons/edge_se3_line.cpp | 17 +- g2o/types/slam3d_addons/edge_se3_line.h | 35 +- g2o/types/slam3d_addons/edge_se3_plane_calib.cpp | 22 +- g2o/types/slam3d_addons/edge_se3_plane_calib.h | 2 +- g2o/types/slam3d_addons/line3d.cpp | 8 +- g2o/types/slam3d_addons/line3d.h | 16 +- g2o/types/slam3d_addons/line3d_test.cpp | 16 +- 81 files changed, 1090 insertions(+), 1174 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc0c4029..8d353c3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,10 +191,10 @@ IF(CMAKE_COMPILER_IS_GNUCXX) #ENDIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # Linux IF(NOT ${ARCH} MATCHES arm AND ${CMAKE_SYSTEM_NAME} MATCHES "Linux") - #SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") - #SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") - SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE}") - SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE}") + #SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") + #SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") + SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE}") + SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE}") ENDIF() # activate warnings !!! SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W") @@ -218,7 +218,7 @@ SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}") # Find Eigen3 SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE}) -FIND_PACKAGE(Eigen3 3.2.1) +FIND_PACKAGE(Eigen3) IF(EIGEN3_FOUND) SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3") ELSE(EIGEN3_FOUND) diff --git a/Makefile b/Makefile index bda1a2ae..d4de880c 100644 --- a/Makefile +++ b/Makefile @@ -18,6 +18,6 @@ clean: build/Makefile build/Makefile: @ echo "Running cmake to generate Makefile"; \ cd build; \ - cmake -DG2O_EIGEN3_INCLUDE:PATH=/home/giorgio/src/aisnavigation/EXTERNAL/eigen3 ../; \ + cmake ../; \ cd - diff --git a/g2o/apps/g2o_hierarchical/CMakeLists.txt b/g2o/apps/g2o_hierarchical/CMakeLists.txt index dd5fd757..35fccd9f 100644 --- a/g2o/apps/g2o_hierarchical/CMakeLists.txt +++ b/g2o/apps/g2o_hierarchical/CMakeLists.txt @@ -27,6 +27,6 @@ SET_TARGET_PROPERTIES(g2o_hierarchical_application PROPERTIES OUTPUT_NAME g2o_hi # ) FILE(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h" "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp") -INSTALL(FILES +INSTALL(FILES ${headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/g2o/apps/g2o_hierarchical) diff --git a/g2o/apps/g2o_hierarchical/backbone_tree_action.cpp b/g2o/apps/g2o_hierarchical/backbone_tree_action.cpp index 8c1ea4e1..41a54f6b 100644 --- a/g2o/apps/g2o_hierarchical/backbone_tree_action.cpp +++ b/g2o/apps/g2o_hierarchical/backbone_tree_action.cpp @@ -6,7 +6,7 @@ namespace g2o { using namespace std; - BackBoneTreeAction::BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step): + BackBoneTreeAction::BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step): _optimizer(optimizer), _vertexTag(vertexTag), _level(level), @@ -14,7 +14,7 @@ namespace g2o { _factory=Factory::instance(); init(); } - + void BackBoneTreeAction::init(){ _vsMap.clear(); _vsMmap.clear(); @@ -27,9 +27,9 @@ namespace g2o { } } - double BackBoneTreeAction::perform(HyperGraph::Vertex* v, - HyperGraph::Vertex* vParent, - HyperGraph::Edge* e, + double BackBoneTreeAction::perform(HyperGraph::Vertex* v, + HyperGraph::Vertex* vParent, + HyperGraph::Edge* e, double distance){ int depth=(int) distance; if (_factory->tag(v)!= _vertexTag) @@ -42,7 +42,7 @@ namespace g2o { } addToMap(parentStar,v); fillStar(parentStar, e); - + // every _step levels you go down in the tree, create a new star if (depth && ! (depth%_step )){ Star* star=new Star(_level+1, _optimizer); @@ -82,7 +82,7 @@ namespace g2o { s->_lowLevelVertices.insert(e->vertices()[i]); } return true; - } + } return false; } } diff --git a/g2o/apps/g2o_hierarchical/backbone_tree_action.h b/g2o/apps/g2o_hierarchical/backbone_tree_action.h index 46f37e4a..4a110f96 100644 --- a/g2o/apps/g2o_hierarchical/backbone_tree_action.h +++ b/g2o/apps/g2o_hierarchical/backbone_tree_action.h @@ -11,19 +11,22 @@ namespace g2o { /** - Dijkstra traversal action that constructs a backbone skeleton on the graph. - It assumes that the traversal contains only edges and vertices belonging to - classes that can be used to construct a backbone. - After a visit is invoked, it returns vector of stars to which the backbone nodes have been assigned - */ + * Dijkstra traversal action that constructs a backbone skeleton on the + * graph. It assumes that the traversal contains only edges and vertices + * belonging to classes that can be used to construct a backbone. After a + * visit is invoked, it returns vector of stars to which the backbone nodes + * have been assigned + */ struct BackBoneTreeAction: public HyperDijkstra::TreeAction{ - //! creates a tree action for constructing the backbone - //! @param optimizer: the optimizer on which the stars are constructed - //! @param vertexTag: the tag of the vertices to use as backbone nodes - //! @param level: the level of the lowLevelEdges of the stars in the backbone - //! @param step: depth in tree after which a new star is initialized + /** + * creates a tree action for constructing the backbone + * @param optimizer: the optimizer on which the stars are constructed + * @param vertexTag: the tag of the vertices to use as backbone nodes + * @param level: the level of the lowLevelEdges of the stars in the backbone + * @param step: depth in tree after which a new star is initialized + */ BackBoneTreeAction(SparseOptimizer* optimizer, std::string vertexTag, int level, int step); - + //! initializes the visit and clears the internal structures void init(); @@ -34,24 +37,28 @@ namespace g2o { //! edges that are not yet assigned to any star inline HyperGraph::EdgeSet& freeEdges() {return _freeEdges;} - //! action to be performed during the descent of the dijkstra tree. - //! it constructs stars according to the dijkstra tree - //! A new star is created every time the depth increases of _step. - //! @param v: the vertex - //! @param vParent: the parent vertex - //! @param e: the edge between v and its parent - //! param distance: the depth in the tree - virtual double perform(HyperGraph::Vertex* v, - HyperGraph::Vertex* vParent, - HyperGraph::Edge* e, + /** + * action to be performed during the descent of the dijkstra tree. it + * constructs stars according to the dijkstra tree A new star is created + * every time the depth increases of _step. + * @param v: the vertex + * @param vParent: the parent vertex + * @param e: the edge between v and its parent + * @param distance: the depth in the tree + */ + virtual double perform(HyperGraph::Vertex* v, + HyperGraph::Vertex* vParent, + HyperGraph::Edge* e, double distance); protected: - //! helper function for adding a vertex to a star. If the vertex is already in _vertexToEdgeMap - //! it replaces the associated star. _vertexStarMultimap is only - //! augmented, thus it contains all associations - //! @param s: the star - //! @param v: the vertex + /** + * helper function for adding a vertex to a star. If the vertex is already + * in _vertexToEdgeMap it replaces the associated star. _vertexStarMultimap + * is only augmented, thus it contains all associations + * @param s: the star + * @param v: the vertex + */ void addToMap(Star* s, HyperGraph::Vertex* v); //! helper function to retrieve the most recent star of a vertex. //! @param v: the vertex @@ -59,7 +66,7 @@ namespace g2o { //! helper function that adds to a star an edge and all its vertices bool fillStar(Star* s, HyperGraph::Edge* e_); - + SparseOptimizer* _optimizer; std::string _vertexTag; int _level; diff --git a/g2o/apps/g2o_hierarchical/edge_creator.h b/g2o/apps/g2o_hierarchical/edge_creator.h index ac4dab87..79c810e4 100644 --- a/g2o/apps/g2o_hierarchical/edge_creator.h +++ b/g2o/apps/g2o_hierarchical/edge_creator.h @@ -8,13 +8,15 @@ namespace g2o { /** - Class that implements a simple edge_creation, based on the types and the ordes of the vertices passed as argument. - Namely, based on an ordered vector of vertices this class implements a method that construct a new edge compatible - with the vertices in the vector. The order of the vector matters. - This class is heavily based on the Factory, and utilizes strings to identify the edge types. + * Class that implements a simple edge_creation, based on the types and the + * ordes of the vertices passed as argument. Namely, based on an ordered + * vector of vertices this class implements a method that construct a new + * edge compatible with the vertices in the vector. The order of the vector + * matters. This class is heavily based on the Factory, and utilizes strings + * to identify the edge types. */ struct EdgeCreator{ - struct EdgeCreatorEntry{ + struct EdgeCreatorEntry { EdgeCreatorEntry(const std::string& edgeTypeName, const std::vector& parameterIds) :_edgeTypeName(edgeTypeName), _parameterIds(parameterIds) {} @@ -33,7 +35,6 @@ struct EdgeCreator{ //! The order matters. //! @param edgeType: the tag of edge to create //! @returns false on failure (incompatible types). Currently returns always true because i did not have time to implement checks - bool addAssociation(const std::string& vertexTypes, const std::string& edgeType); //! Adds an association to the association map diff --git a/g2o/apps/g2o_hierarchical/edge_labeler.cpp b/g2o/apps/g2o_hierarchical/edge_labeler.cpp index e84b3b00..e3a29e01 100644 --- a/g2o/apps/g2o_hierarchical/edge_labeler.cpp +++ b/g2o/apps/g2o_hierarchical/edge_labeler.cpp @@ -14,7 +14,6 @@ namespace g2o { int EdgeLabeler::labelEdges(std::set& edges){ // assume the system is "solved" - // compute the sparse pattern of the inverse std::set > pattern; for (std::set::iterator it=edges.begin(); it!=edges.end(); it++){ @@ -55,7 +54,7 @@ namespace g2o { } } } - + bool EdgeLabeler::computePartialInverse(SparseBlockMatrix& spinv, const std::set >& pattern){ std::vector > blockIndices(pattern.size()); // Why this does not work??? @@ -97,8 +96,8 @@ namespace g2o { for (size_t j=0; jvertices().size(); j++){ const OptimizableGraph::Vertex* vc=(const OptimizableGraph::Vertex*) e->vertices()[j]; int tj = vc->hessianIndex(); - if (tj>-1){ - // cerr << "ti=" << ti << " tj=" << tj + if (tj>-1){ + // cerr << "ti=" << ti << " tj=" << tj // << " cumRow=" << cumRow << " cumCol=" << cumCol << endl; if (ti<=tj){ assert(spinv.block(ti, tj)); @@ -142,8 +141,6 @@ namespace g2o { } assert(smss && "Edge::setMeasurementFromState() not implemented"); - - //std::vector globalPoints(incrementPoints.size()); std::vector > errorPoints(incrementPoints.size()); @@ -163,7 +160,6 @@ namespace g2o { vr->push(); } - for (size_t j=0; jvertices().size(); j++){ OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j]; int tj=vr->hessianIndex(); @@ -186,7 +182,7 @@ namespace g2o { errorPoints[i]._sample=errorPoint; errorPoints[i]._wi=incrementPoints[i]._wi; errorPoints[i]._wp=incrementPoints[i]._wp; - + // pop all the "active" state variables for (size_t j=0; jvertices().size(); j++){ OptimizableGraph::Vertex* vr=(OptimizableGraph::Vertex*) e->vertices()[j]; @@ -198,12 +194,12 @@ namespace g2o { } - // reconstruct the covariance of the error by the sigma points + // reconstruct the covariance of the error by the sigma points MatrixXd errorCov(e->dimension(), e->dimension()); - VectorXd errorMean(e->dimension()); + VectorXd errorMean(e->dimension()); reconstructGaussian(errorMean, errorCov, errorPoints); info=errorCov.inverse(); - + // cerr << "remapped information matrix" << endl; // cerr << info << endl; return true; diff --git a/g2o/apps/g2o_hierarchical/edge_labeler.h b/g2o/apps/g2o_hierarchical/edge_labeler.h index 43a96c1d..b62feb21 100644 --- a/g2o/apps/g2o_hierarchical/edge_labeler.h +++ b/g2o/apps/g2o_hierarchical/edge_labeler.h @@ -6,14 +6,14 @@ namespace g2o { /** - This class implements the functions to label an edge (measurement) based on the actual configuration of the nodes. - It does so by -
    -
  • computing the expected mean of the measurement (_measurement) based on the state variables -
  • computing the joint covariance matrix of all sstate variables on which the measurement depends. -
  • extracting the sigma points from this covariance matrix (which is in the space if the increments used by \oplus -
  • projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge -
+ * This class implements the functions to label an edge (measurement) based + * on the actual configuration of the nodes. It does so by + *
    + *
  • computing the expected mean of the measurement (_measurement) based on the state variables + *
  • computing the joint covariance matrix of all sstate variables on which the measurement depends. + *
  • extracting the sigma points from this covariance matrix (which is in the space if the increments used by \oplus + *
  • projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge + *
*/ struct EdgeLabeler{ //! constructs an edge labeler that operates on the optimizer passed as argument @@ -38,7 +38,7 @@ struct EdgeLabeler{ //! @param pattern: the blocks of the inverse covered by the edge //! @returns true on successm, false on failure, . bool computePartialInverse(SparseBlockMatrix& spinv, const std::set >& pattern); - + //! helper function that labes a specific edge based on the marginals in the sparse block inverse //! @returns true on success, false on failure bool labelEdge( const SparseBlockMatrix& spinv, OptimizableGraph::Edge* e); diff --git a/g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp b/g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp index 20e77771..a4fd4bc9 100644 --- a/g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp +++ b/g2o/apps/g2o_hierarchical/edge_types_cost_function.cpp @@ -4,16 +4,16 @@ namespace g2o { - EdgeTypesCostFunction::EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level): - _edgeTag(edgeTag), - _vertexTag(vertexTag), + EdgeTypesCostFunction::EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level): + _edgeTag(edgeTag), + _vertexTag(vertexTag), _factory(Factory::instance()), _level(level) {} double EdgeTypesCostFunction::operator() (HyperGraph::Edge* e_, HyperGraph::Vertex* from, HyperGraph::Vertex* to){ OptimizableGraph::Edge*e =(OptimizableGraph::Edge*)(e_); - if (e->level()==_level && _factory->tag(e)==_edgeTag && _factory->tag(from)==_vertexTag && _factory->tag(to)==_vertexTag){ + if (e->level()==_level && _factory->tag(e)==_edgeTag && _factory->tag(from)==_vertexTag && _factory->tag(to)==_vertexTag) { return 1.; } return std::numeric_limits::max(); diff --git a/g2o/apps/g2o_hierarchical/edge_types_cost_function.h b/g2o/apps/g2o_hierarchical/edge_types_cost_function.h index e3b42a4c..8eb13d2c 100644 --- a/g2o/apps/g2o_hierarchical/edge_types_cost_function.h +++ b/g2o/apps/g2o_hierarchical/edge_types_cost_function.h @@ -8,15 +8,16 @@ namespace g2o { /** - Cost function for Hyper-Dijkstra that returns 1 when for edges that belong to a given type and maxdouble otherwise. - It can be used to construct a backbone of a hierarchical graph by running Dijkstra. + * Cost function for Hyper-Dijkstra that returns 1 when for edges that belong + * to a given type and maxdouble otherwise. It can be used to construct a + * backbone of a hierarchical graph by running Dijkstra. */ struct EdgeTypesCostFunction: public HyperDijkstra::CostFunction { //! creates a cost function that matches edges at a given level, whose tag is the one given and that are leaving/leading to vertices //! of a selected type. //! @param edgeTag: the tag of the edge type to consider //! @param vertexTag: the tag of the vertex to consider - //! @param level: the level of the edge + //! @param level: the level of the edge EdgeTypesCostFunction(std::string edgeTag, std::string vertexTag, int level); //!cost operator @@ -26,7 +27,6 @@ struct EdgeTypesCostFunction: public HyperDijkstra::CostFunction { std::string _vertexTag; Factory* _factory; int _level; - }; } diff --git a/g2o/apps/g2o_hierarchical/g2o_hierarchical.cpp b/g2o/apps/g2o_hierarchical/g2o_hierarchical.cpp index 0067aee6..5035a254 100644 --- a/g2o/apps/g2o_hierarchical/g2o_hierarchical.cpp +++ b/g2o/apps/g2o_hierarchical/g2o_hierarchical.cpp @@ -1,16 +1,16 @@ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard -// +// // g2o is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published // by the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. -// +// // g2o is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. -// +// // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . @@ -177,12 +177,11 @@ int main(int argc, char** argv) cout << kernels[i] << endl; } } - + AbstractRobustKernelCreator* kernelCreator=0; if (robustKernel.size() > 0) { kernelCreator = RobustKernelFactory::instance()->creator(robustKernel); } - SparseOptimizer optimizer; optimizer.setVerbose(verbose); @@ -215,7 +214,7 @@ int main(int argc, char** argv) } cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl; cerr << "Loaded " << optimizer.edges().size() << " edges" << endl; - + OptimizableGraph::EdgeSet originalEdges=optimizer.edges(); @@ -319,21 +318,21 @@ int main(int argc, char** argv) } break; default: - cerr << "Fatal: unknown backbone type. The largest vertex dimension is: " << poseDim << "." << endl + cerr << "Fatal: unknown backbone type. The largest vertex dimension is: " << poseDim << "." << endl <<"Exiting." << endl; return -1; } // here we need to chop the graph into many lil pieces - + // check for vertices to fix to remove DoF bool gaugeFreedom = optimizer.gaugeFreedom(); OptimizableGraph::Vertex* gauge=optimizer.findGauge(); - - - + + + if (gaugeFreedom) { if (! gauge) { cerr << "# cannot find a vertex to fix in this thing" << endl; @@ -347,7 +346,7 @@ int main(int argc, char** argv) } - + @@ -397,32 +396,32 @@ int main(int argc, char** argv) StarSet stars; - - - computeSimpleStars(stars, &optimizer, &labeler, &creator, - gauge, backboneEdgeType, backboneVertexType, 0, hierarchicalDiameter, - 1, starIterations, uThreshold, debug); - + + + computeSimpleStars(stars, &optimizer, &labeler, &creator, + gauge, backboneEdgeType, backboneVertexType, 0, hierarchicalDiameter, + 1, starIterations, uThreshold, debug); + cerr << "stars computed, stars.size()= " << stars.size() << endl; - + cerr << "hierarchy done, determining border" << endl; EdgeStarMap hesmap; constructEdgeStarMap(hesmap, stars, false); computeBorder(stars, hesmap); - + OptimizableGraph::EdgeSet eset; OptimizableGraph::VertexSet vset; OptimizableGraph::EdgeSet heset; OptimizableGraph::VertexSet hvset; HyperGraph::VertexSet hgauge; - for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){ - + for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++) { + Star* s=*it; if (hgauge.empty()) hgauge=s->gauge(); - for (HyperGraph::VertexSet::iterator git=s->gauge().begin(); - git != s->gauge().end(); git++){ + for (HyperGraph::VertexSet::iterator git=s->gauge().begin(); + git != s->gauge().end(); git++) { hvset.insert(*git); } @@ -440,7 +439,7 @@ int main(int argc, char** argv) } cerr << "eset.size()= " << eset.size() << endl; cerr << "heset.size()= " << heset.size() << endl; - + ofstream starStream("stars.g2o"); optimizer.saveSubset(starStream, eset); starStream.close(); @@ -536,7 +535,7 @@ int main(int argc, char** argv) PropertyMap summary; summary.makeProperty("filename", inputFilename); summary.makeProperty("n_vertices", optimizer.vertices().size()); - + int nLandmarks=0; int nPoses=0; int maxDim = *vertexDimensions.rbegin(); @@ -578,8 +577,8 @@ int main(int argc, char** argv) summary.makeProperty("n_star_h_vertices", hvset.size()); summary.makeProperty("h_initChi", hInitChi); summary.makeProperty("h_finalChi", hFinalChi); - - + + ofstream os; os.open(summaryFile.c_str(), ios::app); summary.writeToCSV(os); diff --git a/g2o/apps/g2o_hierarchical/g2o_hierarchical_test_functions.cpp b/g2o/apps/g2o_hierarchical/g2o_hierarchical_test_functions.cpp index beb1e0ed..f2865ea9 100644 --- a/g2o/apps/g2o_hierarchical/g2o_hierarchical_test_functions.cpp +++ b/g2o/apps/g2o_hierarchical/g2o_hierarchical_test_functions.cpp @@ -57,44 +57,44 @@ void testMarginals(SparseOptimizer& optimizer){ OptimizableGraph::Vertex* v=optimizer.activeVertices()[i]; cerr << "Vertex id:" << v->id() << endl; if (v->hessianIndex()>=0){ - cerr << "increments block :" << v->hessianIndex() << ", " << v->hessianIndex()<< " covariance:" << endl; - VectorXd mean(v->minimalEstimateDimension()); //HACK: need to set identity - mean.fill(0); - VectorXd oldMean(v->minimalEstimateDimension()); //HACK: need to set identity - v->getMinimalEstimateData(&oldMean[0]); - MatrixXd& cov= *(spinv.block(v->hessianIndex(), v->hessianIndex())); - std::vector > spts; - cerr << cov << endl; - if (! sampleUnscented(spts,mean,cov) ) - continue; - - // now apply the oplus operator to the sigma points, - // and get the points in the global space - std::vector > tspts = spts; - - for (size_t j=0; jpush(); - // cerr << "v_before [" << j << "]" << endl; - v->getMinimalEstimateData(&mean[0]); - // cerr << mean << endl; - // cerr << "sigma [" << j << "]" << endl; - // cerr << spts[j]._sample << endl; - v->oplus(&(spts[j]._sample[0])); - v->getMinimalEstimateData(&mean[0]); - tspts[j]._sample=mean; - // cerr << "oplus [" << j << "]" << endl; - // cerr << tspts[j]._sample << endl; - v->pop(); - } - MatrixXd cov2=cov; - reconstructGaussian(mean, cov2, tspts); - cerr << "global block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl; - cerr << "mean: " << endl; - cerr << mean << endl; - cerr << "oldMean: " << endl; - cerr << oldMean << endl; - cerr << "cov: " << endl; - cerr << cov2 << endl; + cerr << "increments block :" << v->hessianIndex() << ", " << v->hessianIndex()<< " covariance:" << endl; + VectorXd mean(v->minimalEstimateDimension()); //HACK: need to set identity + mean.fill(0); + VectorXd oldMean(v->minimalEstimateDimension()); //HACK: need to set identity + v->getMinimalEstimateData(&oldMean[0]); + MatrixXd& cov= *(spinv.block(v->hessianIndex(), v->hessianIndex())); + std::vector > spts; + cerr << cov << endl; + if (! sampleUnscented(spts,mean,cov) ) + continue; + + // now apply the oplus operator to the sigma points, + // and get the points in the global space + std::vector > tspts = spts; + + for (size_t j=0; jpush(); + // cerr << "v_before [" << j << "]" << endl; + v->getMinimalEstimateData(&mean[0]); + // cerr << mean << endl; + // cerr << "sigma [" << j << "]" << endl; + // cerr << spts[j]._sample << endl; + v->oplus(&(spts[j]._sample[0])); + v->getMinimalEstimateData(&mean[0]); + tspts[j]._sample=mean; + // cerr << "oplus [" << j << "]" << endl; + // cerr << tspts[j]._sample << endl; + v->pop(); + } + MatrixXd cov2=cov; + reconstructGaussian(mean, cov2, tspts); + cerr << "global block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl; + cerr << "mean: " << endl; + cerr << mean << endl; + cerr << "oldMean: " << endl; + cerr << oldMean << endl; + cerr << "cov: " << endl; + cerr << cov2 << endl; } // if (v->hessianIndex()>0){ @@ -124,12 +124,12 @@ int unscentedTest(){ cerr << "Point " << i << " " << endl << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl; cerr << spts[i]._sample << endl; } - + VectorXd recMean(6); MatrixXd recCov(6,6); - + reconstructGaussian(recMean, recCov, spts); - + cerr << "recMean" << endl; cerr << recMean << endl; diff --git a/g2o/apps/g2o_hierarchical/simple_star_ops.cpp b/g2o/apps/g2o_hierarchical/simple_star_ops.cpp index 947116b4..0d9f80b7 100644 --- a/g2o/apps/g2o_hierarchical/simple_star_ops.cpp +++ b/g2o/apps/g2o_hierarchical/simple_star_ops.cpp @@ -36,16 +36,16 @@ void constructEdgeStarMap(EdgeStarMap& esmap, StarSet& stars, bool low){ for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){ Star* s = *it; if (low) { - for (HyperGraph::EdgeSet::iterator it = s->lowLevelEdges().begin(); - it!=s->lowLevelEdges().end(); it++){ - HyperGraph::Edge* e=*it; - esmap.insert(make_pair(e,s)); + for (HyperGraph::EdgeSet::iterator it = s->lowLevelEdges().begin(); + it!=s->lowLevelEdges().end(); it++){ + HyperGraph::Edge* e=*it; + esmap.insert(make_pair(e,s)); } } else { - for (HyperGraph::EdgeSet::iterator it = s->starEdges().begin(); - it!=s->starEdges().end(); it++){ - HyperGraph::Edge* e=*it; - esmap.insert(make_pair(e,s)); + for (HyperGraph::EdgeSet::iterator it = s->starEdges().begin(); + it!=s->starEdges().end(); it++){ + HyperGraph::Edge* e=*it; + esmap.insert(make_pair(e,s)); } } } @@ -93,39 +93,38 @@ void assignHierarchicalEdges(StarSet& stars, EdgeStarMap& esmap, EdgeLabeler* la OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit; vertices[1]=v; if (v==vertices[0]) - continue; + continue; HyperGraph::EdgeSet eInSt; int numEdges = vertexEdgesInStar(eInSt, v, s, esmap); - if (Factory::instance()->tag(v)==Factory::instance()->tag(vertices[0]) || numEdges>minNumEdges) { - OptimizableGraph::Edge* e=creator->createEdge(vertices); - //cerr << "creating edge" << e << endl; - if (e) { - e->setLevel(1); - optimizer->addEdge(e); - s->_starEdges.insert(e); - } else { + if (Factory::instance()->tag(v)==Factory::instance()->tag(vertices[0]) || numEdges>minNumEdges) { + OptimizableGraph::Edge* e=creator->createEdge(vertices); + //cerr << "creating edge" << e << endl; + if (e) { + e->setLevel(1); + optimizer->addEdge(e); + s->_starEdges.insert(e); + } else { cerr << "THERE" << endl; - cerr << "FATAL, cannot create edge" << endl; - } + cerr << "FATAL, cannot create edge" << endl; + } } else { - vNew.erase(v); - // cerr << numEdges << " "; - // cerr << "r " << v-> id() << endl; - // remove from the star all edges that are not sufficiently connected - for (HyperGraph::EdgeSet::iterator it=eInSt.begin(); it!=eInSt.end(); it++){ - HyperGraph::Edge* e=*it; - s->lowLevelEdges().erase(e); - } + vNew.erase(v); + // cerr << numEdges << " "; + // cerr << "r " << v-> id() << endl; + // remove from the star all edges that are not sufficiently connected + for (HyperGraph::EdgeSet::iterator it=eInSt.begin(); it!=eInSt.end(); it++){ + HyperGraph::Edge* e=*it; + s->lowLevelEdges().erase(e); + } } } s->lowLevelVertices()=vNew; //cerr << endl; - cerr << "gauge: " << (*s->_gauge.begin())->id() - << " edges:" << s->_lowLevelEdges.size() - << " hedges" << s->_starEdges.size() << endl; - - bool debug = false; - + cerr << "gauge: " << (*s->_gauge.begin())->id() + << " edges:" << s->_lowLevelEdges.size() + << " hedges" << s->_starEdges.size() << endl; + + const bool debug = false; if (debug){ char starLowName[100]; sprintf(starLowName, "star-%04d-low.g2o", starNum); @@ -157,21 +156,21 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ starsInEdge(sset, e, hesmap, s->gauge()); //cerr << "e: " << e << " l:" << e->level() << " sset.size()=" << sset.size() << endl; if (sset.size()>1){ - s->starFrontierEdges().insert(e); + s->starFrontierEdges().insert(e); } } } } - void computeSimpleStars(StarSet& stars, - SparseOptimizer* optimizer, - EdgeLabeler* labeler, - EdgeCreator* creator, - OptimizableGraph::Vertex* gauge_, - std::string edgeTag, - std::string vertexTag, - int level, + void computeSimpleStars(StarSet& stars, + SparseOptimizer* optimizer, + EdgeLabeler* labeler, + EdgeCreator* creator, + OptimizableGraph::Vertex* gauge_, + std::string edgeTag, + std::string vertexTag, + int level, int step, int backboneIterations, int starIterations, @@ -184,14 +183,14 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ EdgeTypesCostFunction f(edgeTag, vertexTag, level); d.shortestPaths(gauge_, &f, - std::numeric_limits< double >::max(), - 1e-6, + std::numeric_limits< double >::max(), + 1e-6, false, std::numeric_limits< double >::max()/2); HyperDijkstra::computeTree(d.adjacencyMap()); // constructs the stars on the backbone - + BackBoneTreeAction bact(optimizer, vertexTag, level, step); bact.init(); @@ -201,8 +200,8 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ d.visitAdjacencyMap(d.adjacencyMap(),&bact,true); stars.clear(); - for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin(); - it!=bact.vertexStarMultiMap().end(); it++){ + for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin(); + it!=bact.vertexStarMultiMap().end(); it++){ stars.insert(it->second); } cerr << "stars.size: " << stars.size() << endl; @@ -213,7 +212,7 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ // for all vertices in the backbone, select all edges leading/leaving from that vertex // that are contained in freeEdges. - + // mark the corresponding "open" vertices and add them to a multimap (vertex->star) // select a gauge in the backbone @@ -228,17 +227,17 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ // push all "open" vertices - // for each open vertex, + // for each open vertex, // compute an initial guess given the backbone // do some rounds of solveDirect // if (fail) // - remove the vertex and the edges in that vertex from the star // - make the structures consistent - + // pop all "open" vertices // pop all "vertices" in the backbone // unfix the vertices in the backbone - + int starNum=0; for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){ Star* s =*it; @@ -247,7 +246,7 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ if (backboneEdges.empty()) continue; - + // cerr << "optimizing backbone" << endl; // one of these should be the gauge, to be simple we select the fisrt one in the backbone OptimizableGraph::VertexSet gauge; @@ -290,7 +289,7 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ // cerr << "optimizing vertices out of bbone" << endl; // cerr << "push" << endl; // cerr << "init" << endl; - s->optimizer()->initializeOptimization(otherEdges); + s->optimizer()->initializeOptimization(otherEdges); // cerr << "guess" << endl; s->optimizer()->computeInitialGuess(); // cerr << "solver init" << endl; @@ -306,30 +305,30 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ } else { cerr << "FATAL: hierarchical thing cannot be used with a solver that does not support the system structure construction" << endl; } - - + + // // then optimize the vertices one at a time to check if a solution is good for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){ OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit); v->solveDirect(); // cerr << " " << d; - // if a solution is found, add a vertex and all the edges in + // if a solution is found, add a vertex and all the edges in //othervertices that are pointing to that edge to the star s->_lowLevelVertices.insert(v); for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){ OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit; if (otherEdges.find(e)!=otherEdges.end()) s->_lowLevelEdges.insert(e); - } + } } //cerr << endl; - + // relax the backbone and optimize it all // cerr << "relax bbone" << endl; s->optimizer()->setFixed(backboneVertices, false); //cerr << "fox gauge bbone" << endl; s->optimizer()->setFixed(s->gauge(),true); - + //cerr << "opt init" << endl; s->optimizer()->initializeOptimization(s->_lowLevelEdges); optimizer->computeActiveErrors(); @@ -355,7 +354,7 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){ //discard the vertices whose error is too big - + OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit); MatrixXd h(v->dimension(), v->dimension()); @@ -375,17 +374,17 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ double d=emin/emax; - + // cerr << " " << d; - if (d>rejectionThreshold){ - // if a solution is found, add a vertex and all the edges in + if (d>rejectionThreshold){ + // if a solution is found, add a vertex and all the edges in //othervertices that are pointing to that edge to the star prunedStarVertices.insert(v); for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){ OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit; if (otherEdges.find(e)!=otherEdges.end()) prunedStarEdges.insert(e); - } + } //cerr << "K( " << v->id() << "," << d << ")" ; vKept ++; } else { @@ -401,7 +400,7 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ //now add to the star the hierarchical edges std::vector vertices(2); vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin(); - + for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){ OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit; vertices[1]=v; @@ -419,15 +418,15 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ } } } - + cerr << " gauge: " << (*s->_gauge.begin())->id() << " kept: " << vKept << " dropped: " << vDropped - << " edges:" << s->_lowLevelEdges.size() - << " hedges" << s->_starEdges.size() - << " initial chi " << initialChi + << " edges:" << s->_lowLevelEdges.size() + << " hedges" << s->_starEdges.size() + << " initial chi " << initialChi << " final chi " << finalchi << endl; - + if (debug) { char starLowName[100]; sprintf(starLowName, "star-%04d-low.g2o", starNum); @@ -435,36 +434,36 @@ void computeBorder(StarSet& stars, EdgeStarMap& hesmap){ optimizer->saveSubset(starLowStream, s->_lowLevelEdges); } bool labelOk=false; - if (!starIterations || starOptResult > 0) - labelOk = s->labelStarEdges(0, labeler); + if (!starIterations || starOptResult > 0) + labelOk = s->labelStarEdges(0, labeler); if (labelOk) { - if (debug) { - char starHighName[100]; - sprintf(starHighName, "star-%04d-high.g2o", starNum); - ofstream starHighStream(starHighName); - optimizer->saveSubset(starHighStream, s->_starEdges); - } + if (debug) { + char starHighName[100]; + sprintf(starHighName, "star-%04d-high.g2o", starNum); + ofstream starHighStream(starHighName); + optimizer->saveSubset(starHighStream, s->_starEdges); + } } else { - cerr << "FAILURE: " << starOptResult << endl; + cerr << "FAILURE: " << starOptResult << endl; } starNum++; - + //label each hierarchical edge s->optimizer()->pop(otherVertices); s->optimizer()->pop(backboneVertices); s->optimizer()->setFixed(s->gauge(),false); } - + StarSet stars2; // now erase the stars that have 0 edges. They r useless for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){ Star* s=*it; if (s->lowLevelEdges().size()==0) { - delete s; + delete s; } else - stars2.insert(s); + stars2.insert(s); } stars=stars2; } diff --git a/g2o/apps/g2o_hierarchical/simple_star_ops.h b/g2o/apps/g2o_hierarchical/simple_star_ops.h index 975c622d..5aadc398 100644 --- a/g2o/apps/g2o_hierarchical/simple_star_ops.h +++ b/g2o/apps/g2o_hierarchical/simple_star_ops.h @@ -23,18 +23,17 @@ namespace g2o { void computeBorder(StarSet& stars, EdgeStarMap& hesmap); - void computeSimpleStars(StarSet& stars, - SparseOptimizer* optimizer, - EdgeLabeler* labeler, - EdgeCreator* creator, - OptimizableGraph::Vertex* gauge_, - std::string edgeTag, std::string vertexTag, - int level, + void computeSimpleStars(StarSet& stars, + SparseOptimizer* optimizer, + EdgeLabeler* labeler, + EdgeCreator* creator, + OptimizableGraph::Vertex* gauge_, + std::string edgeTag, std::string vertexTag, + int level, int step, int backboneIterations=1, int starIterations=30, double rejectionThreshold=1e-5, bool debug=false); - } #endif diff --git a/g2o/apps/g2o_hierarchical/star.cpp b/g2o/apps/g2o_hierarchical/star.cpp index 31838df7..c9fd8e1a 100644 --- a/g2o/apps/g2o_hierarchical/star.cpp +++ b/g2o/apps/g2o_hierarchical/star.cpp @@ -52,7 +52,7 @@ namespace g2o { ok=false; } } else { - optimizer()->initializeOptimization(_lowLevelEdges); + optimizer()->initializeOptimization(_lowLevelEdges); // cerr << "guess" << endl; //optimizer()->computeInitialGuess(); // cerr << "solver init" << endl; @@ -85,7 +85,7 @@ namespace g2o { OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it; v->setFixed(false); } - + return ok; } diff --git a/g2o/apps/g2o_hierarchical/star.h b/g2o/apps/g2o_hierarchical/star.h index d4180902..869af4c0 100644 --- a/g2o/apps/g2o_hierarchical/star.h +++ b/g2o/apps/g2o_hierarchical/star.h @@ -10,17 +10,17 @@ namespace g2o { /** - Class that represents a subgraph in the hierarchical optimization. - The subgraph is consisting of -
    -
  • a set of "central" nodes forming the gauge -
  • a set of edges and vertices in the lower (denser) level -
  • a set of edges in the higher level, each one going from the gauge to one of the vertices in the lower level - These edges form the "star" view at the higher level -
- - Additionally, a star provides a function to compute the parameters for each of the edges in the higher level, based on the actual configuration of the state variables. - It does so by using an EdgeLabeler class. + * Class that represents a subgraph in the hierarchical optimization. + * The subgraph is consisting of + *
    + *
  • a set of "central" nodes forming the gauge + *
  • a set of edges and vertices in the lower (denser) level + *
  • a set of edges in the higher level, each one going from the gauge to one of the vertices in the lower level + * These edges form the "star" view at the higher level + *
+ * Additionally, a star provides a function to compute the parameters for + * each of the edges in the higher level, based on the actual configuration + * of the state variables. It does so by using an EdgeLabeler class. */ struct Star{ @@ -28,12 +28,11 @@ struct Star{ //! @param level: the (higher) level of the star //! @param optimizer: the optimizer Star(int level, SparseOptimizer* optimizer); - + //! labels the edges in the star by first optimizing the low level edges, then by //! calling the labelEdge of the labeler. //! @param iterations: the number of iterations of the optimizer //! @param labeler: the labeler - bool labelStarEdges(int iterations, EdgeLabeler* labeler); //! returns the level of the lower edges in the star @@ -50,13 +49,13 @@ struct Star{ inline HyperGraph::VertexSet& gauge() {return _gauge;} //! set of all vertices in the low level inline HyperGraph::VertexSet& lowLevelVertices() {return _lowLevelVertices;} - + //! level of the star int _level; //! optimizer SparseOptimizer* _optimizer; //! edges in the lower level - HyperGraph::EdgeSet _lowLevelEdges; + HyperGraph::EdgeSet _lowLevelEdges; //! edges in the star HyperGraph::EdgeSet _starEdges; //! edges in the star that lead to some other star diff --git a/g2o/apps/g2o_simulator/convertSegmentLine.cpp b/g2o/apps/g2o_simulator/convertSegmentLine.cpp index 1d55be5a..e1817382 100644 --- a/g2o/apps/g2o_simulator/convertSegmentLine.cpp +++ b/g2o/apps/g2o_simulator/convertSegmentLine.cpp @@ -77,7 +77,7 @@ int main(int argc, char** argv) CommandArgs arg; arg.param("o", outputfilename, "", "output final version of the graph"); arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true); - + arg.parseArgs(argc, argv); OptimizableGraph inGraph; @@ -189,7 +189,7 @@ int main(int argc, char** argv) p1->setId(currentId++); outGraph.addVertex(p1); line->p1Id=p1->id(); - + EdgeLine2DPointXY* p1e=new EdgeLine2DPointXY(); p1e->vertices()[0]=line; p1e->vertices()[1]=p1; @@ -205,7 +205,7 @@ int main(int argc, char** argv) p2->setId(currentId++); outGraph.addVertex(p2); line->p2Id=p2->id(); - + EdgeLine2DPointXY* p2e=new EdgeLine2DPointXY(); p2e->vertices()[0]=line; p2e->vertices()[1]=p2; @@ -231,7 +231,7 @@ int main(int argc, char** argv) Matrix2d p2i=si.block<2,2>(2,2); p2e->setInformation(p2i); outGraph.addEdge(p2e); - + } if (espl) { @@ -284,7 +284,7 @@ int main(int argc, char** argv) } } - + if (outputfilename.size() > 0) { if (outputfilename == "-") { cerr << "saving to stdout"; diff --git a/g2o/apps/g2o_simulator/g2o_anonymize_observations.cpp b/g2o/apps/g2o_simulator/g2o_anonymize_observations.cpp index 3722729c..c7e6e8a1 100644 --- a/g2o/apps/g2o_simulator/g2o_anonymize_observations.cpp +++ b/g2o/apps/g2o_simulator/g2o_anonymize_observations.cpp @@ -75,7 +75,7 @@ int main(int argc, char** argv) { arg.paramLeftOver("graph-output", inputFilename, "", "graph file which will be read", true); arg.parseArgs(argc, argv); OptimizableGraph graph; - + if (inputFilename.size() == 0) { cerr << "No input data specified" << endl; return 0; @@ -97,7 +97,7 @@ int main(int argc, char** argv) { return 2; } } - + for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++){ HyperGraph::Edge* e = *it; if (anonymizeLandmarkEdge(e, graph)) continue; diff --git a/g2o/apps/g2o_simulator/sensor_pose3d.h b/g2o/apps/g2o_simulator/sensor_pose3d.h index 9b3eb2b9..5ce61eb1 100644 --- a/g2o/apps/g2o_simulator/sensor_pose3d.h +++ b/g2o/apps/g2o_simulator/sensor_pose3d.h @@ -32,7 +32,7 @@ namespace g2o { - class G2O_SIMULATOR_API SensorPose3D : public PointSensorParameters, public BinarySensor { + class G2O_SIMULATOR_API SensorPose3D : public PointSensorParameters, public BinarySensor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW SensorPose3D(const std::string& name_); @@ -42,7 +42,7 @@ namespace g2o { void addNoise(EdgeType* e); protected: - + bool isVisible(WorldObjectType* to); int _stepsToIgnore; // these are temporaries diff --git a/g2o/apps/g2o_simulator/sensor_segment2d.cpp b/g2o/apps/g2o_simulator/sensor_segment2d.cpp index 7d2cd99b..354624ca 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d.cpp +++ b/g2o/apps/g2o_simulator/sensor_segment2d.cpp @@ -44,7 +44,7 @@ namespace g2o{ bool SensorSegment2D::isVisible(SensorSegment2D::WorldObjectType* to){ if (! _robotPoseObject) return false; - + assert(to && to->vertex()); VertexType* v=to->vertex(); @@ -52,46 +52,46 @@ namespace g2o{ SE2 iRobot=_robotPoseObject->vertex()->estimate().inverse(); p1 = iRobot * v->estimateP1(); p2 = iRobot * v->estimateP2(); - + Vector3d vp1(p1.x(), p1.y(), 0.); Vector3d vp2(p2.x(), p2.y(), 0.); Vector3d cp=vp1.cross(vp2); // visibility check if (cp[2]<0) return false; - + int circleClip = clipSegmentCircle(p1,p2,sqrt(_maxRange2)); bool clip1=false, clip2=false; switch(circleClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } int fovClip=clipSegmentFov(p1,p2,-_fov, +_fov); switch(fovClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } if (!clip1 && !clip2){ // only if both endpoints have not been clipped do something return true; @@ -114,7 +114,7 @@ namespace g2o{ for (std::set::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){ WorldObjectType* o=dynamic_cast(*it); if (o && isVisible(o)){ - EdgeType* e=mkEdge(o); + EdgeType* e=mkEdge(o); if (e && graph()) { e->setMeasurementFromState(); addNoise(e); diff --git a/g2o/apps/g2o_simulator/sensor_segment2d.h b/g2o/apps/g2o_simulator/sensor_segment2d.h index a9d3412b..b48987e4 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d.h @@ -32,9 +32,9 @@ #include "g2o/types/slam2d_addons/types_slam2d_addons.h" namespace g2o { - + // sensor that senses segments, only if the extremas are visible - class G2O_SIMULATOR_API SensorSegment2D: public PointSensorParameters, public BinarySensor{ + class G2O_SIMULATOR_API SensorSegment2D: public PointSensorParameters, public BinarySensor{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW SensorSegment2D(const std::string& name_); @@ -42,7 +42,7 @@ namespace g2o { virtual void addNoise(EdgeType* e); protected: bool isVisible(WorldObjectType* to); - }; + }; } diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_line.cpp b/g2o/apps/g2o_simulator/sensor_segment2d_line.cpp index a2faac7e..425fe11c 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_line.cpp +++ b/g2o/apps/g2o_simulator/sensor_segment2d_line.cpp @@ -42,7 +42,7 @@ namespace g2o{ bool SensorSegment2DLine::isVisible(SensorSegment2DLine::WorldObjectType* to){ if (! _robotPoseObject) return false; - + assert(to && to->vertex()); VertexType* v=to->vertex(); @@ -50,46 +50,46 @@ namespace g2o{ SE2 iRobot=_robotPoseObject->vertex()->estimate().inverse(); p1 = iRobot * v->estimateP1(); p2 = iRobot * v->estimateP2(); - + Vector3d vp1(p1.x(), p1.y(), 0.); Vector3d vp2(p2.x(), p2.y(), 0.); Vector3d cp=vp1.cross(vp2); // visibility check if (cp[2]<0) return false; - + int circleClip = clipSegmentCircle(p1,p2,sqrt(_maxRange2)); bool clip1=false, clip2=false; switch(circleClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } int fovClip=clipSegmentFov(p1,p2,-_fov, +_fov); switch(fovClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } if (clip1 && clip2) // only if both endpoints have been clipped do something return true; @@ -112,7 +112,7 @@ namespace g2o{ for (std::set::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){ WorldObjectType* o=dynamic_cast(*it); if (o && isVisible(o)){ - EdgeType* e=mkEdge(o); + EdgeType* e=mkEdge(o); if (e && graph()) { e->setMeasurementFromState(); addNoise(e); diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_line.h b/g2o/apps/g2o_simulator/sensor_segment2d_line.h index 66c78465..3008cd7d 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_line.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d_line.h @@ -32,9 +32,9 @@ #include "g2o/types/slam2d_addons/types_slam2d_addons.h" namespace g2o { - + // sensor that senses segments, only if the extremas are visible - class G2O_SIMULATOR_API SensorSegment2DLine: public PointSensorParameters, public BinarySensor{ + class G2O_SIMULATOR_API SensorSegment2DLine: public PointSensorParameters, public BinarySensor{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW SensorSegment2DLine(const std::string& name_); @@ -42,7 +42,7 @@ namespace g2o { virtual void addNoise(EdgeType* e); protected: bool isVisible(WorldObjectType* to); - }; + }; } diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.cpp b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.cpp index 72d0fc75..5e10c060 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.cpp +++ b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.cpp @@ -41,7 +41,7 @@ namespace g2o{ bool SensorSegment2DPointLine::isVisible(SensorSegment2DPointLine::WorldObjectType* to){ if (! _robotPoseObject) return false; - + assert(to && to->vertex()); VertexType* v=to->vertex(); @@ -49,46 +49,46 @@ namespace g2o{ SE2 iRobot=_robotPoseObject->vertex()->estimate().inverse(); p1 = iRobot * v->estimateP1(); p2 = iRobot * v->estimateP2(); - + Vector3d vp1(p1.x(), p1.y(), 0.); Vector3d vp2(p2.x(), p2.y(), 0.); Vector3d cp=vp1.cross(vp2); // visibility check if (cp[2]<0) return false; - + int circleClip = clipSegmentCircle(p1,p2,sqrt(_maxRange2)); bool clip1=false, clip2=false; switch(circleClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } int fovClip=clipSegmentFov(p1,p2,-_fov, +_fov); switch(fovClip){ - case -1: - return false; - case 0: - clip1 = true; - break; - case 1: - clip2 = true; - break; - case 3: - clip1 = true; - clip2 = true; - break; - default:; + case -1: + return false; + case 0: + clip1 = true; + break; + case 1: + clip2 = true; + break; + case 3: + clip1 = true; + clip2 = true; + break; + default:; } if ((clip1 && !clip2)) { _visiblePoint = 1; @@ -116,7 +116,7 @@ namespace g2o{ for (std::set::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){ WorldObjectType* o=dynamic_cast(*it); if (o && isVisible(o)){ - EdgeType* e=mkEdge(o); + EdgeType* e=mkEdge(o); if (e && graph()) { e->setPointNum(_visiblePoint); e->setMeasurementFromState(); diff --git a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h index 558d83f8..62652c74 100644 --- a/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h +++ b/g2o/apps/g2o_simulator/sensor_segment2d_pointline.h @@ -32,9 +32,9 @@ #include "g2o/types/slam2d_addons/types_slam2d_addons.h" namespace g2o { - + // sensor that senses segments, only if the extremas are visible - class G2O_SIMULATOR_API SensorSegment2DPointLine: public PointSensorParameters, public BinarySensor{ + class G2O_SIMULATOR_API SensorSegment2DPointLine: public PointSensorParameters, public BinarySensor{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW SensorSegment2DPointLine(const std::string& name_); @@ -43,7 +43,7 @@ namespace g2o { protected: bool isVisible(WorldObjectType* to); int _visiblePoint; - }; + }; } diff --git a/g2o/apps/g2o_simulator/simulator.h b/g2o/apps/g2o_simulator/simulator.h index 4698cf17..ce092028 100644 --- a/g2o/apps/g2o_simulator/simulator.h +++ b/g2o/apps/g2o_simulator/simulator.h @@ -34,95 +34,95 @@ #include "g2o/stuff/sampler.h" #include "g2o_simulator_api.h" -namespace g2o{ +namespace g2o { class World; class BaseSensor; class G2O_SIMULATOR_API BaseWorldObject{ - public: - BaseWorldObject(World* world_=0) {_world = world_; _vertex=0;} - virtual ~BaseWorldObject(); - void setWorld(World* world_) {_world = world_;} - World* world() {return _world;} - OptimizableGraph* graph(); - OptimizableGraph::Vertex* vertex() {return _vertex;} - virtual void setVertex(OptimizableGraph::Vertex* vertex_); - protected: - OptimizableGraph* _graph; - OptimizableGraph::Vertex* _vertex; - World* _world; + public: + BaseWorldObject(World* world_=0) {_world = world_; _vertex=0;} + virtual ~BaseWorldObject(); + void setWorld(World* world_) {_world = world_;} + World* world() {return _world;} + OptimizableGraph* graph(); + OptimizableGraph::Vertex* vertex() {return _vertex;} + virtual void setVertex(OptimizableGraph::Vertex* vertex_); + protected: + OptimizableGraph* _graph; + OptimizableGraph::Vertex* _vertex; + World* _world; }; template class WorldObject: public BaseWorldObject, VertexType_{ - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef VertexType_ VertexType; - typedef typename VertexType_::EstimateType EstimateType; - WorldObject(World* world_=0): BaseWorldObject(world_){ - _vertex = new VertexType(); - } - virtual void setVertex(OptimizableGraph::Vertex* vertex_){ - if(! dynamic_cast(vertex_)) - return; - _vertex = vertex_; - } - - VertexType* vertex() { - if (! _vertex) return 0; - return dynamic_cast(_vertex); - } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + typedef VertexType_ VertexType; + typedef typename VertexType_::EstimateType EstimateType; + WorldObject(World* world_=0): BaseWorldObject(world_){ + _vertex = new VertexType(); + } + virtual void setVertex(OptimizableGraph::Vertex* vertex_){ + if(! dynamic_cast(vertex_)) + return; + _vertex = vertex_; + } + + VertexType* vertex() { + if (! _vertex) return 0; + return dynamic_cast(_vertex); + } }; class G2O_SIMULATOR_API BaseRobot { - public: - BaseRobot(World* world_, const std::string& name_){_world = world_; _name = name_; } - void setWorld(World* world_) {_world = world_;} - World* world() {return _world;} - const std::string& name() const {return _name;} - OptimizableGraph* graph(); - bool addSensor(BaseSensor* sensor); - const std::set sensors() {return _sensors;} - virtual void sense(); - protected: - World* _world; - std::set _sensors; - std::string _name; + public: + BaseRobot(World* world_, const std::string& name_){_world = world_; _name = name_; } + void setWorld(World* world_) {_world = world_;} + World* world() {return _world;} + const std::string& name() const {return _name;} + OptimizableGraph* graph(); + bool addSensor(BaseSensor* sensor); + const std::set sensors() {return _sensors;} + virtual void sense(); + protected: + World* _world; + std::set _sensors; + std::string _name; }; template class Robot: public BaseRobot{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - public: - typedef RobotPoseObject PoseObject; - typedef std::list TrajectoryType; - typedef typename PoseObject::VertexType VertexType; - typedef typename PoseObject::EstimateType PoseType; - - Robot(World* world_, const std::string& name_): BaseRobot(world_, name_){} - virtual void relativeMove(const PoseType& movement_) { - _pose=_pose*movement_; - move(_pose); - } - - virtual void move(const PoseType& pose_) { - _pose=pose_; - if (world()) { - PoseObject* po=new PoseObject(); - po->vertex()->setEstimate(_pose); - world()->addWorldObject(po); - _trajectory.push_back(po); + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + typedef RobotPoseObject PoseObject; + typedef std::list TrajectoryType; + typedef typename PoseObject::VertexType VertexType; + typedef typename PoseObject::EstimateType PoseType; + + Robot(World* world_, const std::string& name_): BaseRobot(world_, name_){} + virtual void relativeMove(const PoseType& movement_) { + _pose=_pose*movement_; + move(_pose); } - } - - TrajectoryType& trajectory() {return _trajectory;} - const PoseType& pose() const {return _pose;} - protected: - TrajectoryType _trajectory; - PoseType _pose; + + virtual void move(const PoseType& pose_) { + _pose=pose_; + if (world()) { + PoseObject* po=new PoseObject(); + po->vertex()->setEstimate(_pose); + world()->addWorldObject(po); + _trajectory.push_back(po); + } + } + + TrajectoryType& trajectory() {return _trajectory;} + const PoseType& pose() const {return _pose;} + protected: + TrajectoryType _trajectory; + PoseType _pose; }; - + class G2O_SIMULATOR_API BaseSensor{ public: BaseSensor(const std::string& name_){ _name = name_;} @@ -141,153 +141,154 @@ class G2O_SIMULATOR_API BaseSensor{ template class UnarySensor: public BaseSensor { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef RobotType_ RobotType; - typedef typename RobotType::PoseObject PoseObject; - typedef typename RobotType::TrajectoryType TrajectoryType; - typedef typename RobotType::PoseObject::VertexType PoseVertexType; - typedef EdgeType_ EdgeType; - typedef typename EdgeType::InformationType InformationType; - - UnarySensor(const std::string& name): BaseSensor(name) { - _information.setIdentity(); - } - - void setInformation(const InformationType& information_ ) { - _information = information_ ; - _sampler.setDistribution(_information.inverse()); - } - - const InformationType& information() {return _information; } - - virtual void sense() { - _robotPoseObject = 0; - // set the robot pose - if (! robot()) - return; - - RobotType* r =dynamic_cast(robot()); - if (!r) - return; - - if(! r->trajectory().empty()) - _robotPoseObject = *(r->trajectory().rbegin()); - - if (! world() || ! graph()) - return; - - EdgeType* e=mkEdge(); - if (e) { - e->setMeasurementFromState(); - addNoise(e); - graph()->addEdge(e); + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + typedef RobotType_ RobotType; + typedef typename RobotType::PoseObject PoseObject; + typedef typename RobotType::TrajectoryType TrajectoryType; + typedef typename RobotType::PoseObject::VertexType PoseVertexType; + typedef EdgeType_ EdgeType; + typedef typename EdgeType::InformationType InformationType; + + UnarySensor(const std::string& name): BaseSensor(name) { + _information.setIdentity(); + } + + void setInformation(const InformationType& information_ ) { + _information = information_ ; + _sampler.setDistribution(_information.inverse()); } - } + const InformationType& information() {return _information; } - protected: - PoseObject* _robotPoseObject; - InformationType _information; - - EdgeType* mkEdge(){ - PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex(); - EdgeType* e = new EdgeType(); - e->vertices()[0]=robotVertex; - e->information().setIdentity(); - return e; - } - GaussianSampler _sampler; - virtual void addNoise(EdgeType*){}; + virtual void sense() { + _robotPoseObject = 0; + // set the robot pose + if (! robot()) + return; + + RobotType* r =dynamic_cast(robot()); + if (!r) + return; + + if(! r->trajectory().empty()) + _robotPoseObject = *(r->trajectory().rbegin()); + + if (! world() || ! graph()) + return; + + EdgeType* e=mkEdge(); + if (e) { + e->setMeasurementFromState(); + addNoise(e); + graph()->addEdge(e); + } + } + + + protected: + PoseObject* _robotPoseObject; + InformationType _information; + + EdgeType* mkEdge(){ + PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex(); + EdgeType* e = new EdgeType(); + e->vertices()[0]=robotVertex; + e->information().setIdentity(); + return e; + } + GaussianSampler _sampler; + virtual void addNoise(EdgeType*){}; }; template class BinarySensor: public BaseSensor { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef RobotType_ RobotType; - typedef typename RobotType::PoseObject PoseObject; - typedef typename RobotType::TrajectoryType TrajectoryType; - typedef typename RobotType::PoseObject::VertexType PoseVertexType; - typedef EdgeType_ EdgeType; - typedef WorldObjectType_ WorldObjectType; - typedef typename WorldObjectType::VertexType VertexType; - typedef typename EdgeType::InformationType InformationType; - - BinarySensor(const std::string& name): BaseSensor(name) { - _information.setIdentity(); - } - - void setInformation(const InformationType& information_ ) { - _information = information_ ; - _sampler.setDistribution(_information.inverse()); - } - - const InformationType& information() {return _information; } - - virtual void sense() { - _robotPoseObject = 0; - // set the robot pose - if (! robot()) - return; - - RobotType* r =dynamic_cast(robot()); - if (!r) - return; - - if(! r->trajectory().empty()) - _robotPoseObject = *(r->trajectory().rbegin()); - - if (! world() || ! graph()) - return; - - // naive search. just for initial testing - for(std::set::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){ - WorldObjectType * wo = dynamic_cast(*it); - if (wo){ - EdgeType* e=mkEdge(wo); - if (e) { - e->setMeasurementFromState(); - addNoise(e); - graph()->addEdge(e); - } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + typedef RobotType_ RobotType; + typedef typename RobotType::PoseObject PoseObject; + typedef typename RobotType::TrajectoryType TrajectoryType; + typedef typename RobotType::PoseObject::VertexType PoseVertexType; + typedef EdgeType_ EdgeType; + typedef WorldObjectType_ WorldObjectType; + typedef typename WorldObjectType::VertexType VertexType; + typedef typename EdgeType::InformationType InformationType; + + BinarySensor(const std::string& name): BaseSensor(name) { + _information.setIdentity(); + } + + void setInformation(const InformationType& information_ ) { + _information = information_ ; + _sampler.setDistribution(_information.inverse()); + } + + const InformationType& information() {return _information; } + + virtual void sense() { + _robotPoseObject = 0; + // set the robot pose + if (! robot()) + return; + + RobotType* r =dynamic_cast(robot()); + if (!r) + return; + + if(! r->trajectory().empty()) + _robotPoseObject = *(r->trajectory().rbegin()); + + if (! world() || ! graph()) + return; + + // naive search. just for initial testing + for(std::set::iterator it=world()->objects().begin(); it!=world()->objects().end(); it++){ + WorldObjectType * wo = dynamic_cast(*it); + if (wo){ + EdgeType* e=mkEdge(wo); + if (e) { + e->setMeasurementFromState(); + addNoise(e); + graph()->addEdge(e); + } + } } } - } - protected: - PoseObject* _robotPoseObject; - InformationType _information; - - EdgeType* mkEdge(WorldObjectType* object){ - PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex(); - EdgeType* e = new EdgeType(); - e->vertices()[0]=robotVertex; - e->vertices()[1]=object->vertex(); - e->information().setIdentity(); - return e; - } - GaussianSampler _sampler; - virtual void addNoise(EdgeType*){}; + protected: + PoseObject* _robotPoseObject; + InformationType _information; + + EdgeType* mkEdge(WorldObjectType* object){ + PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex(); + EdgeType* e = new EdgeType(); + e->vertices()[0]=robotVertex; + e->vertices()[1]=object->vertex(); + e->information().setIdentity(); + return e; + } + GaussianSampler _sampler; + virtual void addNoise(EdgeType*){}; }; -class G2O_SIMULATOR_API World{ - public: - World(OptimizableGraph* graph_) {_graph = graph_; _runningId=0; _paramId=0;} - OptimizableGraph* graph() {return _graph;} - bool addRobot(BaseRobot* robot); - bool addWorldObject(BaseWorldObject* worldObject); - bool addParameter(Parameter* p); - - std::set& objects() {return _objects;} - std::set& robots() {return _robots; } - protected: - std::set _objects; - std::set _robots; - OptimizableGraph* _graph; - int _runningId; - int _paramId; +class G2O_SIMULATOR_API World +{ + public: + World(OptimizableGraph* graph_) {_graph = graph_; _runningId=0; _paramId=0;} + OptimizableGraph* graph() {return _graph;} + bool addRobot(BaseRobot* robot); + bool addWorldObject(BaseWorldObject* worldObject); + bool addParameter(Parameter* p); + + std::set& objects() {return _objects;} + std::set& robots() {return _robots; } + protected: + std::set _objects; + std::set _robots; + OptimizableGraph* _graph; + int _runningId; + int _paramId; }; } // end namespace diff --git a/g2o/apps/g2o_simulator/simulator2d_segment.cpp b/g2o/apps/g2o_simulator/simulator2d_segment.cpp index d539d331..c1f7a991 100644 --- a/g2o/apps/g2o_simulator/simulator2d_segment.cpp +++ b/g2o/apps/g2o_simulator/simulator2d_segment.cpp @@ -73,7 +73,7 @@ int main(int argc, char** argv) { arg.parseArgs(argc, argv); - + std::tr1::ranlux_base_01 generator; OptimizableGraph graph; World world(&graph); @@ -96,9 +96,9 @@ int main(int argc, char** argv) { th=atan2(sin(th),cos(th)); double xc = ix*(worldSize/segmentGridSize); double yc = iy*(worldSize/segmentGridSize); - + double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator); - + double x1 = xc + cos(th)*l2; double y1 = yc + sin(th)*l2; double x2 = xc - cos(th)*l2; @@ -141,7 +141,7 @@ int main(int argc, char** argv) { poseSensor->setInformation(poseInfo); ss << "-pose"; } - + if (hasPointSensor) { SensorPointXY* pointSensor = new SensorPointXY("pointSensor"); robot.addSensor(pointSensor); @@ -186,9 +186,9 @@ int main(int argc, char** argv) { ss << "-segment2d"; } - - + + robot.move(SE2()); double pStraight=0.7; SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.)); @@ -210,7 +210,7 @@ int main(int argc, char** argv) { if (pose.translation().x() > .5*worldSize){ dtheta = -M_PI; } - + if (pose.translation().y() < -.5*worldSize){ dtheta = M_PI/2; } @@ -242,5 +242,6 @@ int main(int argc, char** argv) { //string fname=outputFilename + ss.str() + ".g2o"; ofstream testStream(outputFilename.c_str()); graph.save(testStream); - + + return 0; } diff --git a/g2o/apps/g2o_simulator/test_simulator2d.cpp b/g2o/apps/g2o_simulator/test_simulator2d.cpp index 4848506a..83eb9665 100644 --- a/g2o/apps/g2o_simulator/test_simulator2d.cpp +++ b/g2o/apps/g2o_simulator/test_simulator2d.cpp @@ -75,7 +75,7 @@ int main(int argc, char** argv) { arg.parseArgs(argc, argv); - + std::tr1::ranlux_base_01 generator; OptimizableGraph graph; World world(&graph); @@ -98,9 +98,9 @@ int main(int argc, char** argv) { th=atan2(sin(th),cos(th)); double xc = ix*(worldSize/segmentGridSize); double yc = iy*(worldSize/segmentGridSize); - + double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator); - + double x1 = xc + cos(th)*l2; double y1 = yc + sin(th)*l2; double x2 = xc - cos(th)*l2; @@ -146,7 +146,7 @@ int main(int argc, char** argv) { poseSensor->setInformation(poseInfo); ss << "-pose"; } - + if (hasPointSensor) { SensorPointXY* pointSensor = new SensorPointXY("pointSensor"); robot.addSensor(pointSensor); @@ -195,7 +195,7 @@ int main(int argc, char** argv) { ss << "-segment2d"; } - + robot.move(SE2()); double pStraight=0.7; SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.)); @@ -217,7 +217,7 @@ int main(int argc, char** argv) { if (pose.translation().x() > .5*worldSize){ dtheta = -M_PI; } - + if (pose.translation().y() < -.5*worldSize){ dtheta = M_PI/2; } @@ -249,5 +249,6 @@ int main(int argc, char** argv) { //string fname=outputFilename + ss.str() + ".g2o"; ofstream testStream(outputFilename.c_str()); graph.save(testStream); - + + return 0; } diff --git a/g2o/core/base_edge.h b/g2o/core/base_edge.h index 738284e0..21e35270 100644 --- a/g2o/core/base_edge.h +++ b/g2o/core/base_edge.h @@ -55,7 +55,7 @@ namespace g2o { virtual ~BaseEdge() {} - virtual double chi2() const + virtual double chi2() const { return _error.dot(information()*_error); } @@ -107,7 +107,7 @@ namespace g2o { template - class BaseEdge<-1,E> : public OptimizableGraph::Edge + class BaseEdge<-1,E> : public OptimizableGraph::Edge { public: @@ -115,14 +115,14 @@ namespace g2o { typedef E Measurement; typedef Matrix ErrorVector; typedef Matrix InformationType; - - BaseEdge() : OptimizableGraph::Edge(){ - + + BaseEdge() : OptimizableGraph::Edge(){ + } virtual ~BaseEdge() {} - virtual double chi2() const + virtual double chi2() const { return _error.dot(information()*_error); } diff --git a/g2o/core/base_multi_edge.h b/g2o/core/base_multi_edge.h index 9f9f6199..1099668b 100644 --- a/g2o/core/base_multi_edge.h +++ b/g2o/core/base_multi_edge.h @@ -108,12 +108,10 @@ namespace g2o { - // PARTIAL TEMPLATE SPECIALIZATION - - + // PARTIAL TEMPLATE SPECIALIZATION template - class G2O_CORE_API BaseMultiEdge<-1,E> : public BaseEdge<-1,E> - { + class G2O_CORE_API BaseMultiEdge<-1,E> : public BaseEdge<-1,E> + { public: /** * \brief helper for mapping the Hessian memory of the upper triangular block @@ -121,7 +119,7 @@ namespace g2o { struct HessianHelper { Map matrix; ///< the mapped memory bool transposed; ///< the block has to be transposed - HessianHelper() : matrix(0, 0, 0), transposed(false) {} + HessianHelper() : matrix(0, 0, 0), transposed(false) {} }; public: @@ -131,12 +129,12 @@ namespace g2o { typedef typename BaseEdge<-1,E>::ErrorVector ErrorVector; typedef typename BaseEdge<-1,E>::InformationType InformationType; typedef Map HessianBlockType; - - BaseMultiEdge() : BaseEdge<-1,E>() - { - // this->_variableSize = true; - } - + + BaseMultiEdge() : BaseEdge<-1,E>() + { + // this->_variableSize = true; + } + virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); /** @@ -144,7 +142,7 @@ namespace g2o { * the result in temporary variable vector _jacobianOplus */ virtual void linearizeOplus(); - + virtual void resize(size_t size); virtual bool allVerticesFixed() const; @@ -168,8 +166,8 @@ namespace g2o { void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + }; #include "base_multi_edge.hpp" diff --git a/g2o/core/base_multi_edge.hpp b/g2o/core/base_multi_edge.hpp index 4c7ba0f8..b6f65741 100644 --- a/g2o/core/base_multi_edge.hpp +++ b/g2o/core/base_multi_edge.hpp @@ -122,7 +122,7 @@ void BaseMultiEdge::linearizeOplus() v->unlockQuadraticForm(); } #endif - + } template @@ -315,7 +315,7 @@ void BaseMultiEdge::computeQuadraticForm(const InformationType& omega, con // v->unlockQuadraticForm(); // } // #endif - + // } // template @@ -509,7 +509,7 @@ void BaseMultiEdge<-1, E>::linearizeOplus() v->unlockQuadraticForm(); } #endif - + } template diff --git a/g2o/core/base_unary_edge.hpp b/g2o/core/base_unary_edge.hpp index 326e6196..bd298286 100644 --- a/g2o/core/base_unary_edge.hpp +++ b/g2o/core/base_unary_edge.hpp @@ -29,6 +29,7 @@ void BaseUnaryEdge::resize(size_t size) { if (size != 1) { std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge::id() << " to " << size << std::endl; + assert(0 && "error resizing unary edge where size != 1"); } BaseEdge::resize(size); } @@ -40,7 +41,7 @@ bool BaseUnaryEdge::allVerticesFixed() const } template -OptimizableGraph::Vertex* BaseUnaryEdge::createVertex(int i) +OptimizableGraph::Vertex* BaseUnaryEdge::createVertex(int i) { if (i!=0) return 0; diff --git a/g2o/core/hyper_graph.cpp b/g2o/core/hyper_graph.cpp index f72f995a..9468854a 100644 --- a/g2o/core/hyper_graph.cpp +++ b/g2o/core/hyper_graph.cpp @@ -31,16 +31,15 @@ namespace g2o { - HyperGraph::Data::Data() { + HyperGraph::Data::Data() { _next = 0; _dataContainer = 0; } HyperGraph::Data::~Data() { - if(_next) - delete _next; - } - + if(_next) + delete _next; + } HyperGraph::Vertex::Vertex(int id) : _id(id) { @@ -129,7 +128,8 @@ namespace g2o { return true; } - bool HyperGraph::setEdgeVertex(HyperGraph::Edge* e, int pos, HyperGraph::Vertex* v){ + bool HyperGraph::setEdgeVertex(HyperGraph::Edge* e, int pos, HyperGraph::Vertex* v) + { Vertex* vOld = e->vertex(pos); if (vOld) vOld->edges().erase(e); @@ -139,8 +139,8 @@ namespace g2o { return true; } - bool HyperGraph::mergeVertices(Vertex* vBig, Vertex* vSmall, bool erase){ - + bool HyperGraph::mergeVertices(Vertex* vBig, Vertex* vSmall, bool erase) + { VertexIDMap::iterator it=_vertices.find(vBig->id()); if (it==_vertices.end()) return false; @@ -148,15 +148,15 @@ namespace g2o { it=_vertices.find(vSmall->id()); if (it==_vertices.end()) return false; - + EdgeSet tmp(vSmall->edges()); bool ok = true; for(EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){ HyperGraph::Edge* e = *it; for (size_t i=0; ivertices().size(); i++){ - Vertex* v=e->vertex(i); - if (v==vSmall) - ok &= setEdgeVertex(e,i,vBig); + Vertex* v=e->vertex(i); + if (v==vSmall) + ok &= setEdgeVertex(e,i,vBig); } } if (erase) @@ -180,7 +180,8 @@ namespace g2o { return true; } - bool HyperGraph::removeVertex(Vertex* v, bool detach) { + bool HyperGraph::removeVertex(Vertex* v, bool detach) + { if (detach){ bool result = detachVertex(v); if (! result) { @@ -195,7 +196,7 @@ namespace g2o { EdgeSet tmp(v->edges()); for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){ if (!removeEdge(*it)){ - assert(0 && __PRETTY_FUNCTION__ && "error in erasing vertex"); + assert(0 && __PRETTY_FUNCTION__ && "error in erasing vertex"); } } _vertices.erase(it); diff --git a/g2o/core/hyper_graph.h b/g2o/core/hyper_graph.h index ba550432..95166e2d 100644 --- a/g2o/core/hyper_graph.h +++ b/g2o/core/hyper_graph.h @@ -77,8 +77,6 @@ namespace g2o { typedef std::bitset GraphElemBitset; - - class G2O_CORE_API Data; class G2O_CORE_API DataContainer; class G2O_CORE_API Vertex; @@ -96,46 +94,45 @@ namespace g2o { HyperGraphElement* clone() const { return 0; } }; - /** * \brief data packet for a vertex. Extend this class to store in the vertices * the potential additional information you need (e.g. images, laser scans, ...). */ class G2O_CORE_API Data : public HyperGraph::HyperGraphElement { - public: - Data(); - ~Data(); - //! read the data from a stream - virtual bool read(std::istream& is) = 0; - //! write the data to a stream - virtual bool write(std::ostream& os) const = 0; - virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;} - inline const Data* next() const {return _next;} - inline Data* next() {return _next;} - inline void setNext(Data* next_) { _next = next_; } - inline DataContainer* dataContainer() { return _dataContainer;} - inline const DataContainer* dataContainer() const { return _dataContainer;} - inline void setDataContainer(DataContainer * dataContainer_){ _dataContainer = dataContainer_;} - protected: - Data* _next; // linked list of multiple data; - DataContainer* _dataContainer; + public: + Data(); + ~Data(); + //! read the data from a stream + virtual bool read(std::istream& is) = 0; + //! write the data to a stream + virtual bool write(std::ostream& os) const = 0; + virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;} + inline const Data* next() const {return _next;} + inline Data* next() {return _next;} + inline void setNext(Data* next_) { _next = next_; } + inline DataContainer* dataContainer() { return _dataContainer;} + inline const DataContainer* dataContainer() const { return _dataContainer;} + inline void setDataContainer(DataContainer * dataContainer_){ _dataContainer = dataContainer_;} + protected: + Data* _next; // linked list of multiple data; + DataContainer* _dataContainer; }; /** - * \brief Container class that implements an interface for adding/removing Data elements in + * \brief Container class that implements an interface for adding/removing Data elements in a linked list */ class G2O_CORE_API DataContainer { - public: - DataContainer() {_userData = 0;} - virtual ~DataContainer() {Data* d=_userData; while (d) {Data* dNext = d->next(); delete d; d=dNext;} } - //! the user data associated with this vertex - const Data* userData() const { return _userData; } - Data* userData() { return _userData; } - void setUserData(Data* obs) { _userData = obs;} - void addUserData(Data* obs) { if (obs) { obs->setNext(_userData); _userData=obs; } } - protected: - Data* _userData; + public: + DataContainer() {_userData = 0;} + virtual ~DataContainer() {Data* d=_userData; while (d) {Data* dNext = d->next(); delete d; d=dNext;} } + //! the user data associated with this vertex + const Data* userData() const { return _userData; } + Data* userData() { return _userData; } + void setUserData(Data* obs) { _userData = obs;} + void addUserData(Data* obs) { if (obs) { obs->setNext(_userData); _userData=obs; } } + protected: + Data* _userData; }; diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp index e62a88ce..df8e50de 100644 --- a/g2o/core/optimizable_graph.cpp +++ b/g2o/core/optimizable_graph.cpp @@ -49,16 +49,6 @@ namespace g2o { using namespace std; -// -// OptimizableGraph::Data::Data(){ -// _next = 0; -// } -// -// OptimizableGraph::Data::~Data(){ -// if (_next) -// delete _next; -// } - OptimizableGraph::Vertex::Vertex() : HyperGraph::Vertex(), @@ -88,7 +78,7 @@ namespace g2o { if (_userData) delete _userData; } - + OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const { return 0; @@ -133,7 +123,6 @@ namespace g2o { HyperGraph::Edge(), _dimension(-1), _level(0), _robustKernel(0) { - // _variableSize = false; } OptimizableGraph::Edge::~Edge() @@ -149,7 +138,7 @@ namespace g2o { return 0; return v->graph(); } - + const OptimizableGraph* OptimizableGraph::Edge::graph() const{ if (! _vertices.size()) return 0; @@ -174,7 +163,7 @@ namespace g2o { cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl; return false; } - + assert (_parameters.size() == _parameterIds.size()); //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl; for (size_t i=0; i<_parameters.size(); i++){ @@ -290,19 +279,19 @@ namespace g2o { if (! e->resolveCaches()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl; return false; - } + } // std::cerr << "updating jacobian size" << std::endl; _jacobianWorkspace.updateSize(e); - + // std::cerr << "about to return true" << std::endl; - + return true; } bool OptimizableGraph::setEdgeVertex(Edge* e, int pos, Vertex* v){ if (! HyperGraph::setEdgeVertex(e,pos,v)){ return false; - } + } if (!e->numUndefinedVertices()){ if (! e->resolveParameters()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl; @@ -311,7 +300,7 @@ namespace g2o { if (! e->resolveCaches()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl; return false; - } + } _jacobianWorkspace.updateSize(e); } return true; @@ -421,7 +410,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) // handle commands encoded in the file bool handledCommand = false; - + if (token == "FIX") { handledCommand = true; int id; @@ -440,7 +429,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) if (handledCommand) continue; - + // do the mapping to an internal type if it matches if (_renamedTypesLookup.size() > 0) { map::const_iterator foundIt = _renamedTypesLookup.find(token); @@ -516,7 +505,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) } if (! to && id2>=0 ){ cerr << __PRETTY_FUNCTION__ << ": Unable to find vertex for edge " << token << " " << id1 << " <-> " << id2 << endl; - delete e; + delete e; e=0; } } @@ -529,7 +518,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) delete e; } else { switch (doInit){ - case 1: + case 1: { HyperGraph::VertexSet fromSet; fromSet.insert(from); @@ -589,7 +578,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) } else { bool r = e->read(currentLine); if (!r || !addEdge(e)) { - cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token; + cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token; for (int l = 0; l < numV; ++l) { if (l > 0) cerr << " <->"; @@ -597,7 +586,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) } delete e; e=0; - } + } } } previousDataContainer = e; @@ -629,7 +618,7 @@ bool OptimizableGraph::load(istream& is, bool createEdges) } } } // while read line - + return true; } @@ -744,7 +733,7 @@ bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset) return os.good(); } - + void OptimizableGraph::addGraph(OptimizableGraph* g){ for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){ OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second); diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h index da6b55d8..d14ac944 100644 --- a/g2o/core/optimizable_graph.h +++ b/g2o/core/optimizable_graph.h @@ -162,7 +162,7 @@ namespace g2o { * Implement setEstimateDataImpl() * @return true on success */ - bool setEstimateData(const std::vector& estimate) { + bool setEstimateData(const std::vector& estimate) { #ifndef NDEBUG int dim = estimateDimension(); assert((dim == -1) || (estimate.size() == std::size_t(dim))); @@ -345,25 +345,19 @@ namespace g2o { virtual bool setMinimalEstimateDataImpl(const double* ) { return false;} }; - + class G2O_CORE_API Edge: public HyperGraph::Edge, public HyperGraph::DataContainer { private: friend struct OptimizableGraph; - //bool _variableSize; public: Edge(); virtual ~Edge(); virtual Edge* clone() const; - //bool _variableSize; - - // tells whether this is a variable size edge (the number of vertices is not known a priori) - //bool variableSize(){return _variableSize;}; - // indicates if all vertices are fixed virtual bool allVerticesFixed() const = 0; - + // computes the error of the edge and stores it in an internal structure virtual void computeError() = 0; @@ -464,7 +458,7 @@ namespace g2o { inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);} inline size_t numParameters() const {return _parameters.size();} inline void resizeParameters(size_t newSize) { - _parameters.resize(newSize, 0); + _parameters.resize(newSize, 0); _parameterIds.resize(newSize, -1); _parameterTypes.resize(newSize, typeid(void*).name()); } @@ -473,7 +467,6 @@ namespace g2o { int _level; RobustKernel* _robustKernel; long long _internalId; - std::vector _cacheIds; template @@ -487,8 +480,8 @@ namespace g2o { } template - void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, - const std::string& _type, + void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, + const std::string& _type, const ParameterVector& parameters); bool resolveParameters(); @@ -511,7 +504,7 @@ namespace g2o { //! adds all edges and vertices of the graph g to this graph. void addGraph(OptimizableGraph* g); - + /** * adds a new vertex. The new vertex is then "taken". * @return false if a vertex with the same id as v is already in the graph, true otherwise. @@ -579,7 +572,7 @@ namespace g2o { //! function provided for convenience, see save() above bool save(const char* filename, int level = 0) const; - + //! save a subgraph to a stream. Again uses the Factory system. bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); @@ -633,7 +626,7 @@ namespace g2o { // helper functions to save an individual vertex bool saveVertex(std::ostream& os, Vertex* v) const; - + // helper function to save an individual parameter bool saveParameter(std::ostream& os, Parameter* v) const; @@ -669,11 +662,11 @@ namespace g2o { ParameterContainer _parameters; JacobianWorkspace _jacobianWorkspace; }; - + /** @} */ - + } // end namespace #endif diff --git a/g2o/examples/plane_slam/plane_test.cpp b/g2o/examples/plane_slam/plane_test.cpp index 6e370a96..ca193517 100644 --- a/g2o/examples/plane_slam/plane_test.cpp +++ b/g2o/examples/plane_slam/plane_test.cpp @@ -42,7 +42,7 @@ int main (/*int argc, char** argv*/){ cerr << "p1 " << p1 << endl; Plane3D p2 = p1; cerr << "p2 " << p2 << endl; - + cerr << "azimuth " << Plane3D::azimuth(p1.normal()) << endl; cerr << "elevation " << Plane3D::elevation(p1.normal()) << endl; Vector3d mv = p2.ominus(p1); @@ -59,7 +59,7 @@ int main (/*int argc, char** argv*/){ mv = p2.ominus(p1); cerr << "p ominus p " << mv[0] << " " << mv[1] << " " << mv[2] << endl; - + Plane3D p3=p1; cerr << "p3 " << p3 << endl; p3.oplus(mv); @@ -86,13 +86,12 @@ int main (/*int argc, char** argv*/){ AngleAxisd r(AngleAxisd(0.0, Vector3d::UnitZ()) * AngleAxisd(0., Vector3d::UnitY()) - * AngleAxisd(0., Vector3d::UnitX())); - + * AngleAxisd(0., Vector3d::UnitX())); + SE3Quat t(r.toRotationMatrix(), Vector3d(0.9,0,0)); cerr << t << endl; transform(l,t); cerr << l << endl; - - + return 0; } diff --git a/g2o/examples/plane_slam/simulator_3d_plane.cpp b/g2o/examples/plane_slam/simulator_3d_plane.cpp index f36c8629..4d71b6dd 100644 --- a/g2o/examples/plane_slam/simulator_3d_plane.cpp +++ b/g2o/examples/plane_slam/simulator_3d_plane.cpp @@ -59,17 +59,17 @@ Vector3d sample_noise_from_plane(const Vector3d& cov ){ return Vector3d(gauss_rand(cov(0)), gauss_rand(cov(1)), gauss_rand(cov(2))); } -struct SimulatorItem{ +struct SimulatorItem { SimulatorItem(OptimizableGraph* graph_): _graph(graph_){} OptimizableGraph* graph() {return _graph;} virtual ~SimulatorItem(){} -protected: + protected: OptimizableGraph* _graph; }; struct WorldItem: public SimulatorItem { - WorldItem(OptimizableGraph* graph_, OptimizableGraph::Vertex* vertex_ = 0) : - SimulatorItem(graph_),_vertex(vertex_) {} + WorldItem(OptimizableGraph* graph_, OptimizableGraph::Vertex* vertex_ = 0) : + SimulatorItem(graph_),_vertex(vertex_) {} OptimizableGraph::Vertex* vertex() {return _vertex;} void setVertex(OptimizableGraph::Vertex* vertex_) {_vertex = vertex_;} protected: @@ -98,7 +98,7 @@ struct Robot: public WorldItem { _planarMotion=false; _position = Isometry3d::Identity(); } - + void move(const Isometry3d& newPosition, int& id) { Isometry3d delta = _position.inverse()*newPosition; @@ -151,7 +151,7 @@ struct Robot: public WorldItem { } Isometry3d _position; - SensorVector _sensors; + SensorVector _sensors; Vector6d _nmovecov; bool _planarMotion; }; @@ -189,7 +189,7 @@ struct PlaneItem: public WorldItem{ p->setId(id); graph()->addVertex(p); setVertex(p); - } + } }; struct PlaneSensor: public Sensor{ @@ -199,7 +199,7 @@ struct PlaneSensor: public Sensor{ _offsetVertex->setEstimate(offset_); robot()->graph()->addVertex(_offsetVertex); }; - + virtual bool isVisible(WorldItem* wi){ if (! wi) return false; @@ -227,7 +227,7 @@ struct PlaneSensor: public Sensor{ Isometry3d sensorPose=robotPose*_offsetVertex->estimate(); VertexPlane* planeVertex=dynamic_cast(pi->vertex()); Plane3D worldPlane=planeVertex->estimate(); - + Plane3D measuredPlane=sensorPose.inverse()*worldPlane; EdgeSE3PlaneSensorCalib* e=new EdgeSE3PlaneSensorCalib(); @@ -245,7 +245,7 @@ struct PlaneSensor: public Sensor{ robot()->graph()->addEdge(e); return true; } - + VertexSE3* _offsetVertex; Vector3d _nplane; }; @@ -306,11 +306,11 @@ int main (int argc , char ** argv){ cerr << "robot" << endl; Robot* r=new Robot(g); - + cerr << "planeSensor" << endl; Matrix3d R=Matrix3d::Identity(); - R << + R << 0, 0, 1, -1, 0, 0, 0, -1, 0; @@ -322,8 +322,8 @@ int main (int argc , char ** argv){ ps->_nplane << 0.03, 0.03, 0.005; r->_sensors.push_back(ps); sim->_robots.push_back(r); - - cerr << "p1" << endl; + + cerr << "p1" << endl; Plane3D plane; PlaneItem* pi =new PlaneItem(g,1); plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.)); @@ -337,7 +337,7 @@ int main (int argc , char ** argv){ pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); - cerr << "p2" << endl; + cerr << "p2" << endl; pi =new PlaneItem(g,3); plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.)); static_cast(pi->vertex())->setEstimate(plane); @@ -379,13 +379,13 @@ int main (int argc , char ** argv){ Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; irelativeMove(0,delta); - else - sim->relativeMove(0,iDelta); - cerr << "s"; - sim->sense(0); + cerr << "m"; + if (a==0) + sim->relativeMove(0,delta); + else + sim->relativeMove(0,iDelta); + cerr << "s"; + sim->sense(0); } } } @@ -400,13 +400,13 @@ int main (int argc , char ** argv){ Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; irelativeMove(0,delta); - else - sim->relativeMove(0,iDelta); - cerr << "s"; - sim->sense(0); + cerr << "m"; + if (a==0) + sim->relativeMove(0,delta); + else + sim->relativeMove(0,iDelta); + cerr << "s"; + sim->sense(0); } } } @@ -419,10 +419,7 @@ int main (int argc , char ** argv){ } else { Vector6d noffcov; noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5; - ps->_offsetVertex->setEstimate( - ps->_offsetVertex->estimate() * - sample_noise_from_se3(noffcov) - ); + ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov)); ps->_offsetVertex->setFixed(false); } @@ -435,7 +432,7 @@ int main (int argc , char ** argv){ // Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix()); // Vector3d tr(1,0,0); // Isometry3d delta; - // delta.matrix().block<3,3>(0,0)=q.toRotationMatrix(); + // delta.matrix().block<3,3>(0,0)=q.toRotationMatrix(); // delta.translation()=tr; // for (size_t i=0; i< g->vertices().size(); i++){ // VertexSE3 *v = dynamic_cast(g->vertex(i)); diff --git a/g2o/stuff/unscented.h b/g2o/stuff/unscented.h index 4d53f826..4d91acbb 100644 --- a/g2o/stuff/unscented.h +++ b/g2o/stuff/unscented.h @@ -76,7 +76,7 @@ namespace g2o { } template - void reconstructGaussian(SampleType& mean, CovarianceType& covariance, + void reconstructGaussian(SampleType& mean, CovarianceType& covariance, const std::vector, Eigen::aligned_allocator > >& sigmaPoints){ mean.fill(0); diff --git a/g2o/types/data/vertex_ellipse.cpp b/g2o/types/data/vertex_ellipse.cpp index 627a7902..d3171cfc 100644 --- a/g2o/types/data/vertex_ellipse.cpp +++ b/g2o/types/data/vertex_ellipse.cpp @@ -83,21 +83,20 @@ namespace g2o { is >> x >> y; addMatchingVertex(x,y); } - + return true; } bool VertexEllipse::write(std::ostream& os) const { - os << _covariance(0,0) << " " << _covariance(0,1) << " " << _covariance(0,2) << " " + os << _covariance(0,0) << " " << _covariance(0,1) << " " << _covariance(0,2) << " " << _covariance(1,1) << " " << _covariance(1,2) << " " << _covariance(2,2) << " "; os << _matchingVertices.size() << " " ; for (int i=0 ; i< _matchingVertices.size(); i++){ - os << _matchingVertices[i].x() << " " << _matchingVertices[i].y() << " "; + os << _matchingVertices[i].x() << " " << _matchingVertices[i].y() << " "; } - return os.good(); } @@ -119,7 +118,7 @@ namespace g2o { return true; } - HyperGraphElementAction* VertexEllipseDrawAction::operator()(HyperGraph::HyperGraphElement* element, + HyperGraphElementAction* VertexEllipseDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; @@ -129,7 +128,7 @@ namespace g2o { return this; } if (_show && !_show->value()) - return this; + return this; VertexEllipse* that = dynamic_cast(element); @@ -145,7 +144,7 @@ namespace g2o { glVertex3f(0,0,0); glVertex3f(x,-y,0); glEnd(); - + glColor3f(0.f,1.f,0.f); for (int i=0; i< that->matchingVertices().size(); i++){ glBegin(GL_LINES); diff --git a/g2o/types/data/vertex_ellipse.h b/g2o/types/data/vertex_ellipse.h index f11ab48e..3bbcfe3a 100644 --- a/g2o/types/data/vertex_ellipse.h +++ b/g2o/types/data/vertex_ellipse.h @@ -35,23 +35,19 @@ namespace g2o { /** * \brief string ellipse to be attached to a vertex - * - * A laser measurement obtained by a robot. The measurement is equipped with a pose of the robot at which - * the measurement was taken. The read/write function correspond to the CARMEN logfile format. */ - -typedef std::vector > myVector2fVector; - class G2O_TYPES_DATA_API VertexEllipse : public RobotData { public: + typedef std::vector > myVector2fVector; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexEllipse(); ~VertexEllipse(); virtual bool write(std::ostream& os) const; virtual bool read(std::istream& is); - + const Eigen::Matrix3f& covariance() {return _covariance;} void setCovariance( Eigen::Matrix3f& c) { _covariance = c; _updateSVD();} const Eigen::Matrix2f& U() {return _U;} @@ -62,9 +58,9 @@ typedef std::vector > Eigen::Vector2f v(x,y); _matchingVertices.push_back(v); } - + void clearMatchingVertices(){_matchingVertices.clear();} - + const std::vector& matchingVerticesIDs() {return _matchingVerticesIDs;} void addMatchingVertexID(int id){ _matchingVerticesIDs.push_back(id); @@ -80,7 +76,7 @@ typedef std::vector > myVector2fVector _matchingVertices; }; - #ifdef G2O_HAVE_OPENGL +#ifdef G2O_HAVE_OPENGL class G2O_TYPES_DATA_API VertexEllipseDrawAction: public DrawAction{ public: VertexEllipseDrawAction(); diff --git a/g2o/types/slam2d/edge_se2.cpp b/g2o/types/slam2d/edge_se2.cpp index ddb81491..b1c39175 100644 --- a/g2o/types/slam2d/edge_se2.cpp +++ b/g2o/types/slam2d/edge_se2.cpp @@ -163,7 +163,7 @@ namespace g2o { glColor3f(POSE_EDGE_GHOST_COLOR); toTransform = to->estimate(); fromTransform = to->estimate()*e->measurement().inverse(); - // DRAW THE FROM EDGE AS AN ARROW + // DRAW THE FROM EDGE AS AN ARROW glPushMatrix(); glTranslatef(fromTransform.translation().x(),fromTransform.translation().y(),0.f); glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f); diff --git a/g2o/types/slam2d/edge_se2_lotsofxy.cpp b/g2o/types/slam2d/edge_se2_lotsofxy.cpp index 12dc4c5a..42c005d6 100644 --- a/g2o/types/slam2d/edge_se2_lotsofxy.cpp +++ b/g2o/types/slam2d/edge_se2_lotsofxy.cpp @@ -5,147 +5,146 @@ #include "g2o/stuff/opengl_primitives.h" #endif - namespace g2o{ - + EdgeSE2LotsOfXY::EdgeSE2LotsOfXY() : BaseMultiEdge<-1,Eigen::VectorXd>(){ resize(0); } - - + + void EdgeSE2LotsOfXY::computeError(){ VertexSE2 * pose = static_cast (_vertices[0]); for(unsigned int i=0; i<_observedPoints; i++){ VertexPointXY * xy = static_cast (_vertices[1+i]); Eigen::Vector2d m = pose->estimate().inverse() * xy->estimate(); - + unsigned int index = 2*i; _error[index] = m[0] - _measurement[index]; _error[index+1] = m[1] - _measurement[index+1]; } - + } - + bool EdgeSE2LotsOfXY::read(std::istream& is){ is >> _observedPoints; setSize(_observedPoints + 1); - + // read the measurements for(unsigned int i=0; i<_observedPoints; i++){ unsigned int index = 2*i; is >> _measurement[index] >> _measurement[index+1]; } - + // read the information matrix for(unsigned int i=0; i<_observedPoints*2; i++){ // fill the "upper triangle" part of the matrix for(unsigned int j=i; j<_observedPoints*2; j++){ - is >> information()(i,j); + is >> information()(i,j); } - + // fill the lower triangle part for(unsigned int j=0; j(_vertices[0]); const double& x1 = vi->estimate().translation()[0]; const double& y1 = vi->estimate().translation()[1]; const double& th1 = vi->estimate().rotation().angle(); - + double ct = cos(th1) ; double st = sin(th1) ; - + Eigen::MatrixXd Ji; unsigned int rows = 2*(_vertices.size()-1); Ji.resize(rows, 3); Ji.fill(0); - + Eigen::Matrix2d poseRot; // inverse of the rotation matrix associated to the pose poseRot << ct , st , - -st , ct ; - + -st , ct ; + Eigen::Matrix2d minusPoseRot = -poseRot; - - + + for(unsigned int i=1; i<_vertices.size(); i++){ g2o::VertexPointXY * point = (g2o::VertexPointXY *) (_vertices[i]); - + const double& x2 = point->estimate()[0]; const double& y2 = point->estimate()[1]; - + unsigned int index = 2*(i-1); - + Ji.block<2,2>(index,0) = minusPoseRot; - + Ji(index,2) = ct * (y2-y1) + st * (x1 - x2); Ji(index+1,2) = st * (y1-y2) + ct * (x1 - x2); - - + + Eigen::MatrixXd Jj; Jj.resize(rows, 2); Jj.fill(0); Jj.block<2,2>(index, 0) = poseRot; - + _jacobianOplus[i] = Jj; } _jacobianOplus[0] = Ji; } - - + + void EdgeSE2LotsOfXY::initialEstimate(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate){ (void) toEstimate; - + assert(initialEstimatePossible(fixed, toEstimate) && "Bad vertices specified"); - + VertexSE2 * pose = static_cast(_vertices[0]); - + bool estimate_this[_observedPoints]; for(unsigned int i=0; i<_observedPoints; i++){ estimate_this[i] = true; } - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ for(unsigned int i=1; i<_vertices.size(); i++){ - VertexPointXY * vert = static_cast(_vertices[i]); - if(vert->id() == (*it)->id()) estimate_this[i-1] = false; - } + VertexPointXY * vert = static_cast(_vertices[i]); + if(vert->id() == (*it)->id()) estimate_this[i-1] = false; + } } - + for(unsigned int i=1; i<_vertices.size(); i++){ if(estimate_this[i-1]){ - unsigned int index = 2*(i-1); - Eigen::Vector2d submeas(_measurement[index], _measurement[index+1]); - VertexPointXY * vert = static_cast(_vertices[i]); - vert->setEstimate(pose->estimate() * submeas); + unsigned int index = 2*(i-1); + Eigen::Vector2d submeas(_measurement[index], _measurement[index+1]); + VertexPointXY * vert = static_cast(_vertices[i]); + vert->setEstimate(pose->estimate() * submeas); } } } @@ -153,30 +152,30 @@ namespace g2o{ double EdgeSE2LotsOfXY::initialEstimatePossible(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate){ (void) toEstimate; - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ if(_vertices[0]->id() == (*it)->id()){ - return 1.0; + return 1.0; } } - + return -1.0; } - - + + bool EdgeSE2LotsOfXY::setMeasurementFromState(){ VertexSE2 * pose = static_cast (_vertices[0]); - + for(unsigned int i=0; i<_observedPoints; i++){ VertexPointXY * xy = static_cast (_vertices[1+i]); Eigen::Vector2d m = pose->estimate().inverse() * xy->estimate(); - + unsigned int index = 2*i; _measurement[index] = m[0]; _measurement[index+1] = m[1]; } - + return true; } - + } // end namespace g2o diff --git a/g2o/types/slam2d/edge_se2_lotsofxy.h b/g2o/types/slam2d/edge_se2_lotsofxy.h index c4b5594d..e3b74948 100644 --- a/g2o/types/slam2d/edge_se2_lotsofxy.h +++ b/g2o/types/slam2d/edge_se2_lotsofxy.h @@ -8,44 +8,42 @@ #include "vertex_point_xy.h" namespace g2o{ - - class G2O_TYPES_SLAM2D_API EdgeSE2LotsOfXY : public BaseMultiEdge<-1,Eigen::VectorXd>{ - - unsigned int _observedPoints; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - EdgeSE2LotsOfXY(); - - void setDimension(int dimension_){ - _dimension = dimension_; - _information.resize(dimension_, dimension_); - _error.resize(dimension_, 1); - _measurement.resize(dimension_, 1); - } - - void setSize(int vertices){ - resize(vertices); - _observedPoints = vertices-1; - setDimension(_observedPoints*2); - } - - virtual void computeError(); - - virtual bool read(std::istream& is); - virtual bool write(std::ostream& os) const; - - virtual bool setMeasurementFromState(); - - virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); - virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); - - virtual void linearizeOplus(); - }; -} + class G2O_TYPES_SLAM2D_API EdgeSE2LotsOfXY : public BaseMultiEdge<-1,Eigen::VectorXd> + { + protected: + unsigned int _observedPoints; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EdgeSE2LotsOfXY(); + + void setDimension(int dimension_){ + _dimension = dimension_; + _information.resize(dimension_, dimension_); + _error.resize(dimension_, 1); + _measurement.resize(dimension_, 1); + } + + void setSize(int vertices){ + resize(vertices); + _observedPoints = vertices-1; + setDimension(_observedPoints*2); + } + virtual void computeError(); + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + virtual bool setMeasurementFromState(); + + virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + + virtual void linearizeOplus(); + }; + +} #endif // G2O_EDGE_SE2_LOTSOF_XY diff --git a/g2o/types/slam2d/edge_se2_twopointsxy.cpp b/g2o/types/slam2d/edge_se2_twopointsxy.cpp index 10e47460..5bd7bb20 100644 --- a/g2o/types/slam2d/edge_se2_twopointsxy.cpp +++ b/g2o/types/slam2d/edge_se2_twopointsxy.cpp @@ -11,22 +11,22 @@ namespace g2o{ EdgeSE2TwoPointsXY::EdgeSE2TwoPointsXY() : BaseMultiEdge<4,Eigen::Vector4d>(){ resize(3); } - + void EdgeSE2TwoPointsXY::computeError(){ VertexSE2 * pose = static_cast (_vertices[0]); VertexPointXY * xy1 = static_cast (_vertices[1]); VertexPointXY * xy2 = static_cast (_vertices[2]); - - + + Eigen::Vector2d m1 = pose->estimate().inverse() * xy1->estimate(); Eigen::Vector2d m2 = pose->estimate().inverse() * xy2->estimate(); - + _error[0] = m1[0] - _measurement[0]; _error[1] = m1[1] - _measurement[1]; _error[2] = m2[0] - _measurement[2]; _error[3] = m2[1] - _measurement[3]; } - + bool EdgeSE2TwoPointsXY::read(std::istream& is){ is >> _measurement[0] >> _measurement[1] >> _measurement[2] >> _measurement[3]; is >> information()(0,0) >> information()(0,1) >> information()(0,2) >> information()(0,3) >> information()(1,1) >> information()(1,2) >> information()(1,3) >> information()(2,2) >> information()(2,3) >> information()(3,3); @@ -38,40 +38,40 @@ namespace g2o{ information()(3,2) = information()(2,3); return true; } - + bool EdgeSE2TwoPointsXY::write(std::ostream& os) const{ os << measurement()[0] << " " << measurement()[1] << " " << measurement()[2] << " " << measurement()[3] << " "; os << information()(0,0) << " " << information()(0,1) << " " << information()(0,2) << " " << information()(0,3) << " " << information()(1,1) << " " << information()(1,2) << " " << information()(1,3) << " " << information()(2,2) << " " << information()(2,3) << " " << information()(3,3); return os.good(); } - - + + void EdgeSE2TwoPointsXY::initialEstimate(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate){ (void) toEstimate; - + assert(initialEstimatePossible(fixed, toEstimate) && "Bad vertices specified"); - + VertexSE2 * pose = static_cast(_vertices[0]); VertexPointXY * v1 = static_cast(_vertices[1]); VertexPointXY * v2 = static_cast(_vertices[2]); - + bool estimatev1 = true; bool estimatev2 = true; - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ if(v1->id() == (*it)->id()){ - estimatev1 = false; + estimatev1 = false; } else if(v2->id() == (*it)->id()){ - estimatev2 = false; + estimatev2 = false; } } - + if(estimatev1){ Eigen::Vector2d submeas(_measurement[0], _measurement[1]); v1->setEstimate(pose->estimate() * submeas); } - + if(estimatev2){ Eigen::Vector2d submeas(_measurement[2], _measurement[3]); v2->setEstimate(pose->estimate() * submeas); @@ -81,15 +81,15 @@ namespace g2o{ double EdgeSE2TwoPointsXY::initialEstimatePossible(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate){ (void) toEstimate; - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ if(_vertices[0]->id() == (*it)->id()){ - return 1.0; + return 1.0; } } - + return -1.0; - + // if(fixed.size() == 2) return 1.0; // if(fixed.size() < 2){ // // if the fixed vertex is the position, it is possible to estimate the position of the two XY vertices @@ -103,24 +103,24 @@ namespace g2o{ // } // return -1.0; } - - + + bool EdgeSE2TwoPointsXY::setMeasurementFromState(){ VertexSE2 * pose = static_cast (_vertices[0]); VertexPointXY * xy1 = static_cast (_vertices[1]); VertexPointXY * xy2 = static_cast (_vertices[2]); - - + + Eigen::Vector2d m1 = pose->estimate().inverse() * xy1->estimate(); Eigen::Vector2d m2 = pose->estimate().inverse() * xy2->estimate(); - + _measurement[0] = m1[0]; _measurement[1] = m1[1]; _measurement[2] = m2[0]; _measurement[3] = m2[1]; - - + + return true; } - + } // end namespace g2o diff --git a/g2o/types/slam2d/edge_se2_twopointsxy.h b/g2o/types/slam2d/edge_se2_twopointsxy.h index 03d999ba..c70aa54c 100644 --- a/g2o/types/slam2d/edge_se2_twopointsxy.h +++ b/g2o/types/slam2d/edge_se2_twopointsxy.h @@ -9,21 +9,21 @@ namespace g2o{ - class G2O_TYPES_SLAM2D_API EdgeSE2TwoPointsXY : public BaseMultiEdge<4, Eigen::Vector4d>{ - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - EdgeSE2TwoPointsXY(); - - virtual void computeError(); - -virtual bool read(std::istream& is); - virtual bool write(std::ostream& os) const; - - virtual bool setMeasurementFromState(); - - virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); - virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + class G2O_TYPES_SLAM2D_API EdgeSE2TwoPointsXY : public BaseMultiEdge<4, Eigen::Vector4d> + { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EdgeSE2TwoPointsXY(); + + virtual void computeError(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual bool setMeasurementFromState(); + + virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); }; } #endif // G2O_EDGE_SE2_TWOPOINTS_XY_H diff --git a/g2o/types/slam2d/se2.h b/g2o/types/slam2d/se2.h index 8654b4c4..d072ff08 100644 --- a/g2o/types/slam2d/se2.h +++ b/g2o/types/slam2d/se2.h @@ -43,9 +43,9 @@ namespace g2o { EIGEN_MAKE_ALIGNED_OPERATOR_NEW; SE2():_R(0),_t(0,0){} - SE2(const Eigen::Isometry2d& iso): _R(0), _t(iso.translation()){ - _R.fromRotationMatrix(iso.linear()); - } + SE2(const Eigen::Isometry2d& iso): _R(0), _t(iso.translation()){ + _R.fromRotationMatrix(iso.linear()); + } SE2(const Vector3d& v):_R(v[2]),_t(v[0],v[1]){} @@ -111,10 +111,10 @@ namespace g2o { } inline Eigen::Isometry2d toIsometry() const { - Eigen::Isometry2d iso = Eigen::Isometry2d::Identity(); - iso.linear() = _R.toRotationMatrix(); - iso.translation() = _t; - return iso; + Eigen::Isometry2d iso = Eigen::Isometry2d::Identity(); + iso.linear() = _R.toRotationMatrix(); + iso.translation() = _t; + return iso; } protected: diff --git a/g2o/types/slam2d/types_slam2d.cpp b/g2o/types/slam2d/types_slam2d.cpp index 29a72485..20d23547 100644 --- a/g2o/types/slam2d/types_slam2d.cpp +++ b/g2o/types/slam2d/types_slam2d.cpp @@ -51,7 +51,7 @@ namespace g2o { G2O_REGISTER_TYPE(EDGE_POINTXY, EdgePointXY); G2O_REGISTER_TYPE(EDGE_SE2_TWOPOINTSXY, EdgeSE2TwoPointsXY); G2O_REGISTER_TYPE(EDGE_SE2_LOTSOFXY, EdgeSE2LotsOfXY); - + G2O_REGISTER_ACTION(VertexSE2WriteGnuplotAction); G2O_REGISTER_ACTION(VertexPointXYWriteGnuplotAction); diff --git a/g2o/types/slam2d/vertex_point_xy.cpp b/g2o/types/slam2d/vertex_point_xy.cpp index 772a2179..9188943a 100644 --- a/g2o/types/slam2d/vertex_point_xy.cpp +++ b/g2o/types/slam2d/vertex_point_xy.cpp @@ -95,11 +95,10 @@ namespace g2o { refreshPropertyPtrs(params); if (! _previousParams) return this; - + if (_show && !_show->value()) return this; VertexPointXY* that = static_cast(element); - glPushMatrix(); glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT); diff --git a/g2o/types/slam2d/vertex_point_xy.h b/g2o/types/slam2d/vertex_point_xy.h index 7be90a18..db9512f8 100644 --- a/g2o/types/slam2d/vertex_point_xy.h +++ b/g2o/types/slam2d/vertex_point_xy.h @@ -47,31 +47,31 @@ namespace g2o { } virtual bool setEstimateDataImpl(const double* est){ - _estimate[0] = est[0]; - _estimate[1] = est[1]; - return true; + _estimate[0] = est[0]; + _estimate[1] = est[1]; + return true; } virtual bool getEstimateData(double* est) const{ - est[0] = _estimate[0]; - est[1] = _estimate[1]; - return true; + est[0] = _estimate[0]; + est[1] = _estimate[1]; + return true; } - + virtual int estimateDimension() const { - return 2; + return 2; } virtual bool setMinimalEstimateDataImpl(const double* est){ - return setEstimateData(est); + return setEstimateData(est); } virtual bool getMinimalEstimateData(double* est) const{ - return getEstimateData(est); + return getEstimateData(est); } - + virtual int minimalEstimateDimension() const { - return 2; + return 2; } virtual void oplusImpl(const double* update) diff --git a/g2o/types/slam2d/vertex_se2.cpp b/g2o/types/slam2d/vertex_se2.cpp index 41c649b6..4fcc4ace 100644 --- a/g2o/types/slam2d/vertex_se2.cpp +++ b/g2o/types/slam2d/vertex_se2.cpp @@ -64,7 +64,7 @@ namespace g2o { std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid output stream specified" << std::endl; return 0; } - + VertexSE2* v = static_cast(element); *(params->os) << v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; @@ -90,7 +90,7 @@ namespace g2o { } - HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element, + HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; @@ -99,7 +99,7 @@ namespace g2o { if (! _previousParams) return this; - + if (_show && !_show->value()) return this; diff --git a/g2o/types/slam2d_addons/CMakeLists.txt b/g2o/types/slam2d_addons/CMakeLists.txt index b657680c..7dedc48f 100644 --- a/g2o/types/slam2d_addons/CMakeLists.txt +++ b/g2o/types/slam2d_addons/CMakeLists.txt @@ -10,7 +10,6 @@ ADD_LIBRARY(types_slam2d_addons ${G2O_LIB_TYPE} edge_line2d_pointxy.cpp edge_line2d_pointxy.h edge_line2d.cpp edge_line2d.h g2o_types_slam2d_addons_api.h - ) SET_TARGET_PROPERTIES(types_slam2d_addons PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_slam2d_addons) diff --git a/g2o/types/slam2d_addons/edge_line2d.h b/g2o/types/slam2d_addons/edge_line2d.h index a24b3028..fda2647d 100644 --- a/g2o/types/slam2d_addons/edge_line2d.h +++ b/g2o/types/slam2d_addons/edge_line2d.h @@ -33,7 +33,7 @@ #include "types_slam2d_addons.h" namespace g2o { - + using namespace Eigen; class G2O_TYPES_SLAM2D_API EdgeLine2D : public BaseBinaryEdge<2, Line2D, VertexLine2D, VertexLine2D> diff --git a/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp b/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp index ee76167f..8e9c642e 100644 --- a/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp +++ b/g2o/types/slam2d_addons/edge_line2d_pointxy.cpp @@ -129,7 +129,7 @@ namespace g2o { // #ifdef G2O_HAVE_OPENGL // EdgeLine2DPointXYDrawAction::EdgeLine2DPointXYDrawAction(): DrawAction(typeid(EdgeLine2DPointXY).name()){} -// HyperGraphElementAction* EdgeLine2DPointXYDrawAction::operator()(HyperGraph::HyperGraphElement* element, +// HyperGraphElementAction* EdgeLine2DPointXYDrawAction::operator()(HyperGraph::HyperGraphElement* element, // HyperGraphElementAction::Parameters* params_){ // if (typeid(*element).name()!=_typeName) // return 0; @@ -137,7 +137,7 @@ namespace g2o { // refreshPropertyPtrs(params_); // if (! _previousParams) // return this; - + // if (_show && !_show->value()) // return this; diff --git a/g2o/types/slam2d_addons/edge_line2d_pointxy.h b/g2o/types/slam2d_addons/edge_line2d_pointxy.h index 28da92b8..f4c5c9d6 100644 --- a/g2o/types/slam2d_addons/edge_line2d_pointxy.h +++ b/g2o/types/slam2d_addons/edge_line2d_pointxy.h @@ -52,7 +52,7 @@ namespace g2o { } virtual bool setMeasurementData(const double* d){ - _measurement = *d; + _measurement = *d; return true; } @@ -60,7 +60,7 @@ namespace g2o { *d = _measurement; return true; } - + virtual int measurementDimension() const {return 1;} virtual bool setMeasurementFromState(){ diff --git a/g2o/types/slam2d_addons/edge_se2_line2d.cpp b/g2o/types/slam2d_addons/edge_se2_line2d.cpp index 32737719..fbebbf14 100644 --- a/g2o/types/slam2d_addons/edge_se2_line2d.cpp +++ b/g2o/types/slam2d_addons/edge_se2_line2d.cpp @@ -130,7 +130,7 @@ namespace g2o { // #ifdef G2O_HAVE_OPENGL // EdgeSE2Line2DDrawAction::EdgeSE2Line2DDrawAction(): DrawAction(typeid(EdgeSE2Line2D).name()){} -// HyperGraphElementAction* EdgeSE2Line2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, +// HyperGraphElementAction* EdgeSE2Line2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, // HyperGraphElementAction::Parameters* params_){ // if (typeid(*element).name()!=_typeName) // return 0; @@ -138,7 +138,7 @@ namespace g2o { // refreshPropertyPtrs(params_); // if (! _previousParams) // return this; - + // if (_show && !_show->value()) // return this; diff --git a/g2o/types/slam2d_addons/edge_se2_line2d.h b/g2o/types/slam2d_addons/edge_se2_line2d.h index 8a13c868..5ee1826d 100644 --- a/g2o/types/slam2d_addons/edge_se2_line2d.h +++ b/g2o/types/slam2d_addons/edge_se2_line2d.h @@ -67,7 +67,7 @@ namespace g2o { d[1] = _measurement[1]; return true; } - + virtual int measurementDimension() const {return 2;} virtual bool setMeasurementFromState(){ diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d.cpp b/g2o/types/slam2d_addons/edge_se2_segment2d.cpp index a7b23b21..5034f900 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d.cpp +++ b/g2o/types/slam2d_addons/edge_se2_segment2d.cpp @@ -133,7 +133,7 @@ namespace g2o { // #ifdef G2O_HAVE_OPENGL // EdgeSE2Segment2DDrawAction::EdgeSE2Segment2DDrawAction(): DrawAction(typeid(EdgeSE2Segment2D).name()){} -// HyperGraphElementAction* EdgeSE2Segment2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, +// HyperGraphElementAction* EdgeSE2Segment2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, // HyperGraphElementAction::Parameters* params_){ // if (typeid(*element).name()!=_typeName) // return 0; @@ -141,7 +141,7 @@ namespace g2o { // refreshPropertyPtrs(params_); // if (! _previousParams) // return this; - + // if (_show && !_show->value()) // return this; diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d.h b/g2o/types/slam2d_addons/edge_se2_segment2d.h index 9024fc38..a2e5ae14 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d.h @@ -54,7 +54,7 @@ namespace g2o { Map error2(&_error(2)); SE2 iEst=v1->estimate().inverse(); error1 = (iEst * l2->estimateP1()); - error2 = (iEst * l2->estimateP2()); + error2 = (iEst * l2->estimateP2()); _error = _error - _measurement; } @@ -69,7 +69,7 @@ namespace g2o { data = _measurement; return true; } - + virtual int measurementDimension() const {return 4;} virtual bool setMeasurementFromState(){ diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h index ad5dd977..c197aa84 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_line.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_line.h @@ -75,7 +75,7 @@ namespace g2o { data = _measurement; return true; } - + virtual int measurementDimension() const {return 2;} virtual bool setMeasurementFromState(){ diff --git a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h index 6bbc835b..ba8a8b15 100644 --- a/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h +++ b/g2o/types/slam2d_addons/edge_se2_segment2d_pointLine.h @@ -78,7 +78,7 @@ namespace g2o { data = _measurement; return true; } - + virtual int measurementDimension() const {return 3;} virtual bool setMeasurementFromState(){ @@ -102,7 +102,7 @@ namespace g2o { protected: int _pointNum; - + /* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */ /* virtual void linearizeOplus(); */ /* #endif */ diff --git a/g2o/types/slam2d_addons/types_slam2d_addons.cpp b/g2o/types/slam2d_addons/types_slam2d_addons.cpp index ec6a0748..f753fdd1 100644 --- a/g2o/types/slam2d_addons/types_slam2d_addons.cpp +++ b/g2o/types/slam2d_addons/types_slam2d_addons.cpp @@ -39,16 +39,16 @@ namespace g2o { G2O_REGISTER_TYPE(VERTEX_SEGMENT2D, VertexSegment2D); G2O_REGISTER_TYPE(VERTEX_LINE2D, VertexLine2D); - G2O_REGISTER_TYPE(EDGE_SE2_SEGMENT2D, EdgeSE2Segment2D); + G2O_REGISTER_TYPE(EDGE_SE2_SEGMENT2D, EdgeSE2Segment2D); G2O_REGISTER_TYPE(EDGE_SE2_SEGMENT2D_LINE, EdgeSE2Segment2DLine); G2O_REGISTER_TYPE(EDGE_SE2_SEGMENT2D_POINTLINE, EdgeSE2Segment2DPointLine); G2O_REGISTER_TYPE(EDGE_SE2_LINE2D, EdgeSE2Line2D); G2O_REGISTER_TYPE(EDGE_LINE2D, EdgeLine2D); G2O_REGISTER_TYPE(EDGE_LINE2D_POINTXY, EdgeLine2DPointXY); - + #ifdef G2O_HAVE_OPENGL G2O_REGISTER_ACTION(VertexSegment2DDrawAction); G2O_REGISTER_ACTION(VertexLine2DDrawAction); - #endif + } // end namespace diff --git a/g2o/types/slam2d_addons/vertex_line2d.cpp b/g2o/types/slam2d_addons/vertex_line2d.cpp index 8abe0ffe..1cf8d98c 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.cpp +++ b/g2o/types/slam2d_addons/vertex_line2d.cpp @@ -74,7 +74,7 @@ namespace g2o { // std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl; // return false; // } - + // VertexLine2D* v = static_cast(element); // *(params->os) << v->estimate().x() << " " << v->estimate().y() << std::endl; // return this; @@ -94,7 +94,7 @@ namespace g2o { return true; } - HyperGraphElementAction* VertexLine2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, + HyperGraphElementAction* VertexLine2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_ ){ if (typeid(*element).name()!=_typeName) @@ -106,7 +106,7 @@ namespace g2o { if (_show && !_show->value()) return this; - + VertexLine2D* that = static_cast(element); glPushAttrib(GL_CURRENT_BIT | GL_BLEND); @@ -120,7 +120,7 @@ namespace g2o { VertexPointXY *vp1=0, *vp2=0; vp1=dynamic_cast (that->graph()->vertex(that->p1Id)); vp2=dynamic_cast (that->graph()->vertex(that->p2Id)); - + glColor4f(0.8f,0.5f,0.3f,0.3f); if (vp1 && vp2) { glColor4f(0.8f,0.5f,0.3f,0.7f); diff --git a/g2o/types/slam2d_addons/vertex_line2d.h b/g2o/types/slam2d_addons/vertex_line2d.h index 1946e6e0..370fa6d9 100644 --- a/g2o/types/slam2d_addons/vertex_line2d.h +++ b/g2o/types/slam2d_addons/vertex_line2d.h @@ -44,13 +44,13 @@ namespace g2o { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexLine2D(); - + double theta() const {return _estimate[0]; } void setTheta(double t) { _estimate[0] = t; } double rho() const {return _estimate[1]; } void setRho(double r) { _estimate[1] = r; } - + virtual void setToOriginImpl() { _estimate.setZero(); } @@ -66,8 +66,8 @@ namespace g2o { v=_estimate; return true; } - - virtual int estimateDimension() const { + + virtual int estimateDimension() const { return 2; } @@ -78,8 +78,8 @@ namespace g2o { virtual bool getMinimalEstimateData(double* est) const{ return getEstimateData(est); } - - virtual int minimalEstimateDimension() const { + + virtual int minimalEstimateDimension() const { return 2; } @@ -92,9 +92,6 @@ namespace g2o { virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; int p1Id, p2Id; - - protected: - }; /* class G2O_TYPES_SLAM2D_API VertexLine2DWriteGnuplotAction: public WriteGnuplotAction { */ diff --git a/g2o/types/slam2d_addons/vertex_segment2d.cpp b/g2o/types/slam2d_addons/vertex_segment2d.cpp index de1e11bd..b133370a 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.cpp +++ b/g2o/types/slam2d_addons/vertex_segment2d.cpp @@ -43,13 +43,13 @@ #include "g2o/stuff/macros.h" namespace g2o { - + VertexSegment2D::VertexSegment2D() : BaseVertex<4, Vector4d>() { _estimate.setZero(); } - + bool VertexSegment2D::read(std::istream& is) { for (size_t i=0; i<4; i++) @@ -75,7 +75,7 @@ namespace g2o { std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl; return 0; } - + VertexSegment2D* v = static_cast(element); *(params->os) << v->estimateP1().x() << " " << v->estimateP1().y() << std::endl; *(params->os) << v->estimateP2().x() << " " << v->estimateP2().y() << std::endl; @@ -97,7 +97,7 @@ namespace g2o { return true; } - HyperGraphElementAction* VertexSegment2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, + HyperGraphElementAction* VertexSegment2DDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_ ){ if (typeid(*element).name()!=_typeName) @@ -109,7 +109,7 @@ namespace g2o { if (_show && !_show->value()) return this; - + VertexSegment2D* that = static_cast(element); glColor3f(0.8f,0.5f,0.3f); diff --git a/g2o/types/slam2d_addons/vertex_segment2d.h b/g2o/types/slam2d_addons/vertex_segment2d.h index efaa4339..00904445 100644 --- a/g2o/types/slam2d_addons/vertex_segment2d.h +++ b/g2o/types/slam2d_addons/vertex_segment2d.h @@ -63,8 +63,7 @@ namespace g2o { return true; } - - virtual int estimateDimension() const { + virtual int estimateDimension() const { return 4; } @@ -75,8 +74,8 @@ namespace g2o { virtual bool getMinimalEstimateData(double* est) const{ return getEstimateData(est); } - - virtual int minimalEstimateDimension() const { + + virtual int minimalEstimateDimension() const { return 4; } @@ -94,7 +93,7 @@ namespace g2o { class G2O_TYPES_SLAM2D_ADDONS_API VertexSegment2DWriteGnuplotAction: public WriteGnuplotAction { public: VertexSegment2DWriteGnuplotAction(); - virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, + virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_); }; @@ -102,7 +101,7 @@ namespace g2o { class G2O_TYPES_SLAM2D_ADDONS_API VertexSegment2DDrawAction: public DrawAction{ public: VertexSegment2DDrawAction(); - virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, + virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_); protected: FloatProperty *_pointSize; diff --git a/g2o/types/slam3d/edge_se3_lotsofxyz.cpp b/g2o/types/slam3d/edge_se3_lotsofxyz.cpp index fb46cbf3..75d0d1ae 100644 --- a/g2o/types/slam3d/edge_se3_lotsofxyz.cpp +++ b/g2o/types/slam3d/edge_se3_lotsofxyz.cpp @@ -12,17 +12,17 @@ namespace g2o{ resize(0); } - + bool EdgeSE3LotsOfXYZ::setMeasurementFromState(){ VertexSE3 * pose = static_cast (_vertices[0]); - + Eigen::Transform poseinv = pose->estimate().inverse(); - + for(unsigned int i=0; i<_observedPoints; i++){ VertexPointXYZ * xyz = static_cast (_vertices[1+i]); // const Eigen::Vector3d &pt = xyz->estimate(); Eigen::Vector3d m = poseinv * xyz->estimate(); - + unsigned int index = 3*i; _measurement[index] = m[0]; _measurement[index+1] = m[1]; @@ -30,39 +30,39 @@ namespace g2o{ } return true; } - - - + + + void EdgeSE3LotsOfXYZ::computeError(){ VertexSE3 * pose = static_cast (_vertices[0]); - + for(unsigned int i=0; i<_observedPoints; i++){ VertexPointXYZ * xyz = static_cast (_vertices[1+i]); Eigen::Vector3d m = pose->estimate().inverse() * xyz->estimate(); - + unsigned int index = 3*i; _error[index] = m[0] - _measurement[index]; _error[index+1] = m[1] - _measurement[index+1]; _error[index+2] = m[2] - _measurement[index+2]; } } - - - + + + // void EdgeSE3LotsOfXYZ::linearizeOplus(){ - + // // BaseMultiEdge::linearizeOplus(); // g2o::VertexSE3 * pose = (g2o::VertexSE3 *) (_vertices[0]); - + // // initialize Ji matrix // Eigen::MatrixXd Ji; // unsigned int rows = 3*(_vertices.size()-1); // Ji.resize(rows, 6); // Ji.fill(0); - + // // Eigen::Matrix3d poseRot = pose->estimate().inverse().rotation(); - + // double p[7]; // pose->getEstimateData(p); // double ex=p[3]; @@ -74,9 +74,9 @@ namespace g2o{ // (2*(w*w + ex*ex) - 1), (2*(ex*ey + w*ez)), (2*(ex*ez - w*ey)), // (2*(ex*ey - w*ez)), (2*(w*w + ey*ey) - 1), (2*(ey*ez + w*ex)), // (2*(ex*ez + w*ey)), (2*(ey*ez - w*ex)), (2*(w*w + ez*ez) - 1); - + // // std::cout << "poseRot:" << std::endl << poseRot << std::endl << "A:" << std::endl << A << std::endl; - + // Eigen::Matrix3d dAx; // Eigen::Matrix3d dAy; // Eigen::Matrix3d dAz; @@ -84,159 +84,159 @@ namespace g2o{ // 4*ex, 2*ey, 2*ez, // 2*ey, 0, 2*w, // 2*ez, -2*w, 0; - + // dAy << // 0, 2*ex, -2*w, // 2*ex, 4*ey, 2*ez, // 2*w, 2*ez, 0; - + // dAz << // 0, 2*w, 2*ey, // -2*w, 0, 2*ey, // 2*ex, 2*ey, 4*ez; - + // // Eigen::MatrixXd Jg; // // Jg.resize(rows, 6); // // Jg.fill(0); - - + + // for(unsigned int i=1; i<_vertices.size(); i++){ // unsigned int index=3*(i-1); // row index in the jacobian - + // g2o::VertexPointXYZ * point = (g2o::VertexPointXYZ *) (_vertices[i]); // double l[3]; // point->getEstimateData(l); - + // // Ji.block<3,3>(index,0) = -A; // Ji.block<3,3>(index,0) = -Eigen::Matrix3d::Identity(); // this is because the oPlus method considers the relative displacement - + // Ji(index, 3) = dAx(0,0) * (l[0] - p[0]) + dAx(0,1) * (l[1] - p[1]) + dAx(0,2) * (l[2] - p[2]); // Ji(index, 4) = dAy(0,0) * (l[0] - p[0]) + dAy(0,1) * (l[1] - p[1]) + dAy(0,2) * (l[2] - p[2]); // Ji(index, 5) = dAz(0,0) * (l[0] - p[0]) + dAz(0,1) * (l[1] - p[1]) + dAz(0,2) * (l[2] - p[2]); - + // Ji(index+1, 3) = dAx(1,0) * (l[0] - p[0]) + dAx(1,1) * (l[1] - p[1]) + dAx(1,2) * (l[2] - p[2]); // Ji(index+1, 4) = dAy(1,0) * (l[0] - p[0]) + dAy(1,1) * (l[1] - p[1]) + dAy(1,2) * (l[2] - p[2]); // Ji(index+1, 5) = dAz(1,0) * (l[0] - p[0]) + dAz(1,1) * (l[1] - p[1]) + dAz(1,2) * (l[2] - p[2]); - + // Ji(index+2, 3) = dAx(2,0) * (l[0] - p[0]) + dAx(2,1) * (l[1] - p[1]) + dAx(2,2) * (l[2] - p[2]); // Ji(index+2, 4) = dAy(2,0) * (l[0] - p[0]) + dAy(2,1) * (l[1] - p[1]) + dAy(2,2) * (l[2] - p[2]); // Ji(index+2, 5) = dAz(2,0) * (l[0] - p[0]) + dAz(2,1) * (l[1] - p[1]) + dAz(2,2) * (l[2] - p[2]); - + // // Eigen::Vector3d Zcam = pose->estimate().inverse() * point->estimate(); - + // // Jg.block<3,3>(index,0) = -Eigen::Matrix3d::Identity(); - + // // Jg(index, 3) = -0.0; // // Jg(index, 4) = -2*Zcam(2); // // Jg(index, 5) = 2*Zcam(1); - + // // Jg(index+1, 3) = 2*Zcam(2); // // Jg(index+1, 4) = -0.0; // // Jg(index+1, 5) = -2*Zcam(0); - + // // Jg(index+2, 3) = -2*Zcam(1); // // Jg(index+2, 4) = 2*Zcam(0); // // Jg(index+2, 5) = -0.0; - + // Eigen::MatrixXd Jj; // Jj.resize(rows, 3); // Jj.fill(0); // Jj.block<3,3>(index,0) = A; - + // _jacobianOplus[i] = Jj; // } - + // // std::cout << "numerically computed jacobian:" << std::endl << _jacobianOplus[0] << std::endl << "'my' jacobian:" << std::endl << Ji << std::endl << "num - mine" << std::endl << _jacobianOplus[0] - Ji << std::endl << std::endl; // // std::cout << "Giorgio's style jacobian:" << std::endl << Jg << std::endl << "Jg - Ji:" << std::endl << Jg-Ji << std::endl << "num - Jg:" << std::endl << _jacobianOplus[0] - Jg << std::endl << std::endl; // _jacobianOplus[0] = Ji; - + // } - - + + void EdgeSE3LotsOfXYZ::linearizeOplus(){ - + g2o::VertexSE3 * pose = (g2o::VertexSE3 *) (_vertices[0]); - + // initialize Ji matrix Eigen::MatrixXd Ji; unsigned int rows = 3*(_vertices.size()-1); Ji.resize(rows, 6); Ji.fill(0); - + Eigen::Matrix3d poseRot = pose->estimate().inverse().rotation(); - + for(unsigned int i=1; i<_vertices.size(); i++){ g2o::VertexPointXYZ * point = (g2o::VertexPointXYZ *) (_vertices[i]); Eigen::Vector3d Zcam = pose->estimate().inverse() * point->estimate(); - + unsigned int index=3*(i-1); - + // Ji.block<3,3>(index,0) = -poseRot; Ji.block<3,3>(index,0) = -Eigen::Matrix3d::Identity(); - + Ji(index, 3) = -0.0; Ji(index, 4) = -2*Zcam(2); Ji(index, 5) = 2*Zcam(1); - + Ji(index+1, 3) = 2*Zcam(2); Ji(index+1, 4) = -0.0; Ji(index+1, 5) = -2*Zcam(0); - + Ji(index+2, 3) = -2*Zcam(1); Ji(index+2, 4) = 2*Zcam(0); Ji(index+2, 5) = -0.0; - + Eigen::MatrixXd Jj; Jj.resize(rows, 3); Jj.fill(0); Jj.block<3,3>(index,0) = poseRot; - + _jacobianOplus[i] = Jj; } - + _jacobianOplus[0] = Ji; - + } - + bool EdgeSE3LotsOfXYZ::read(std::istream& is){ is >> _observedPoints; setSize(_observedPoints + 1); - + // read the measurements for(unsigned int i=0; i<_observedPoints; i++){ unsigned int index = 3*i; is >> _measurement[index] >> _measurement[index+1] >> _measurement[index+2]; } - + // read the information matrix for(unsigned int i=0; i<_observedPoints*3; i++){ // fill the "upper triangle" part of the matrix for(unsigned int j=i; j<_observedPoints*3; j++){ is >> information()(i,j); } - + // fill the lower triangle part for(unsigned int j=0; j(_vertices[0]); - + bool estimate_this[_observedPoints]; for(unsigned int i=0; i<_observedPoints; i++){ estimate_this[i] = true; } - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ for(unsigned int i=1; i<_vertices.size(); i++){ VertexPointXYZ * vert = static_cast(_vertices[i]); if(vert->id() == (*it)->id()) estimate_this[i-1] = false; - } + } } - + for(unsigned int i=1; i<_vertices.size(); i++){ if(estimate_this[i-1]){ unsigned int index = 3*(i-1); @@ -276,18 +276,18 @@ namespace g2o{ } } } - - - + + + double EdgeSE3LotsOfXYZ::initialEstimatePossible(const OptimizableGraph::VertexSet& fixed, OptimizableGraph::Vertex* toEstimate){ (void) toEstimate; - + for(std::set::iterator it=fixed.begin(); it!=fixed.end(); it++){ if(_vertices[0]->id() == (*it)->id()){ return 1.0; } } - + return -1.0; } } diff --git a/g2o/types/slam3d/edge_se3_lotsofxyz.h b/g2o/types/slam3d/edge_se3_lotsofxyz.h index f865d47e..b47bded3 100644 --- a/g2o/types/slam3d/edge_se3_lotsofxyz.h +++ b/g2o/types/slam3d/edge_se3_lotsofxyz.h @@ -10,41 +10,42 @@ namespace g2o{ class G2O_TYPES_SLAM3D_API EdgeSE3LotsOfXYZ : public BaseMultiEdge<-1, Eigen::VectorXd>{ - - unsigned int _observedPoints; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + protected: + unsigned int _observedPoints; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeSE3LotsOfXYZ(); - - void setDimension(int dimension_){ - _dimension = dimension_; - _information.resize(dimension_, dimension_); - _error.resize(dimension_, 1); - _measurement.resize(dimension_, 1); - } - - void setSize(int vertices){ - resize(vertices); - _observedPoints = vertices-1; - setDimension(_observedPoints*3); - } - - - virtual void computeError(); - - virtual bool read(std::istream& is); - virtual bool write(std::ostream& os) const; - - virtual bool setMeasurementFromState(); - - virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); - virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); - - virtual void linearizeOplus(); - + + void setDimension(int dimension_){ + _dimension = dimension_; + _information.resize(dimension_, dimension_); + _error.resize(dimension_, 1); + _measurement.resize(dimension_, 1); + } + + void setSize(int vertices){ + resize(vertices); + _observedPoints = vertices-1; + setDimension(_observedPoints*3); + } + + + virtual void computeError(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual bool setMeasurementFromState(); + + virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + virtual double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*); + + virtual void linearizeOplus(); + }; - + } diff --git a/g2o/types/slam3d/edge_se3_offset.cpp b/g2o/types/slam3d/edge_se3_offset.cpp index 91ec554f..6fe3075c 100644 --- a/g2o/types/slam3d/edge_se3_offset.cpp +++ b/g2o/types/slam3d/edge_se3_offset.cpp @@ -64,12 +64,12 @@ namespace g2o { return false; Vector7d meas; - for (int i=0; i<7; i++) + for (int i=0; i<7; i++) is >> meas[i]; // normalize the quaternion to recover numerical precision lost by storing as human readable text Vector4d::MapType(meas.data()+3).normalize(); setMeasurement(internal::fromVectorQT(meas)); - + if (is.bad()) { return false; } @@ -82,7 +82,7 @@ namespace g2o { if (is.bad()) { // we overwrite the information matrix with the Identity information().setIdentity(); - } + } return true; } @@ -133,7 +133,7 @@ namespace g2o { VertexSE3 *to = static_cast(_vertices[1]); Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse(); - + if (from_.count(from) > 0) { to->setEstimate(from->estimate() * virtualMeasurement); } else diff --git a/g2o/types/slam3d/types_slam3d.cpp b/g2o/types/slam3d/types_slam3d.cpp index 3556d70d..796cddc6 100644 --- a/g2o/types/slam3d/types_slam3d.cpp +++ b/g2o/types/slam3d/types_slam3d.cpp @@ -1,16 +1,16 @@ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard -// +// // g2o is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published // by the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. -// +// // g2o is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. -// +// // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . @@ -41,9 +41,9 @@ namespace g2o { G2O_REGISTER_TYPE(EDGE_PROJECT_DEPTH, EdgeSE3PointXYZDepth); G2O_REGISTER_TYPE(EDGE_POINTXYZ, EdgePointXYZ); - + G2O_REGISTER_TYPE(EDGE_SE3_LOTSOF_XYZ, EdgeSE3LotsOfXYZ) - + /*********** ACTIONS ************/ G2O_REGISTER_ACTION(VertexSE3WriteGnuplotAction); G2O_REGISTER_ACTION(VertexPointXYZWriteGnuplotAction); diff --git a/g2o/types/slam3d_addons/CMakeLists.txt b/g2o/types/slam3d_addons/CMakeLists.txt index 9567b475..f8cba18a 100644 --- a/g2o/types/slam3d_addons/CMakeLists.txt +++ b/g2o/types/slam3d_addons/CMakeLists.txt @@ -15,9 +15,9 @@ ADD_LIBRARY(types_slam3d_addons ${G2O_LIB_TYPE} # edge_se3_plane_nm_calib.h line3d.cpp line3d.h vertex_line3d.cpp vertex_line3d.h - edge_se3_line.cpp edge_se3_line.h + edge_se3_line.cpp edge_se3_line.h edge_line3d.cpp edge_line3d.h - edge_plane.cpp edge_plane.h + edge_plane.cpp edge_plane.h edge_se3_calib.cpp edge_se3_calib.h types_slam3d_addons.cpp types_slam3d_addons.h diff --git a/g2o/types/slam3d_addons/edge_se3_calib.cpp b/g2o/types/slam3d_addons/edge_se3_calib.cpp index d04c04d4..fc9db195 100644 --- a/g2o/types/slam3d_addons/edge_se3_calib.cpp +++ b/g2o/types/slam3d_addons/edge_se3_calib.cpp @@ -54,7 +54,7 @@ namespace Slam3dAddons { bool EdgeSE3Calib::read(std::istream& is) { Vector7d meas; - for (int i=0; i<7; i++) + for (int i=0; i<7; i++) is >> meas[i]; // normalize the quaternion to recover numerical precision lost by storing as human readable text Vector4d::MapType(meas.data()+3).normalize(); @@ -72,7 +72,7 @@ namespace Slam3dAddons { if (is.bad()) { // we overwrite the information matrix with the Identity information().setIdentity(); - } + } return true; } diff --git a/g2o/types/slam3d_addons/edge_se3_euler.cpp b/g2o/types/slam3d_addons/edge_se3_euler.cpp index 8e7ed870..1db3fb34 100644 --- a/g2o/types/slam3d_addons/edge_se3_euler.cpp +++ b/g2o/types/slam3d_addons/edge_se3_euler.cpp @@ -1,16 +1,16 @@ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard -// +// // g2o is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published // by the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. -// +// // g2o is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. -// +// // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . @@ -24,9 +24,8 @@ namespace Slam3dAddons { void jac_quat3_euler3(Eigen::Matrix& J, const Isometry3d& t) { - Vector7d t0 = g2o::internal::toVectorQT(t); - + double delta=1e-6; double idelta= 1. / (2. * delta); @@ -69,7 +68,7 @@ void jac_quat3_euler3(Eigen::Matrix& J, const Isometry3d& t) Vector6d meas = g2o::internal::toVectorET(_measurement); for (int i=0; i<6; i++) os << meas[i] << " "; - + Matrix J; jac_quat3_euler3(J, measurement()); //HACK: invert the jacobian to simulate the inverse derivative diff --git a/g2o/types/slam3d_addons/edge_se3_euler.h b/g2o/types/slam3d_addons/edge_se3_euler.h index 7780dc1e..c6d997cf 100644 --- a/g2o/types/slam3d_addons/edge_se3_euler.h +++ b/g2o/types/slam3d_addons/edge_se3_euler.h @@ -1,16 +1,16 @@ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard -// +// // g2o is free software: you can redistribute it and/or modify // it under the terms of the GNU Lesser General Public License as published // by the Free Software Foundation, either version 3 of the License, or // (at your option) any later version. -// +// // g2o is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU Lesser General Public License for more details. -// +// // You should have received a copy of the GNU Lesser General Public License // along with this program. If not, see . @@ -30,10 +30,10 @@ namespace Slam3dAddons { /** - * \brief 3D edge between two VertexSE3, uses the euler angle parameterization - for the read/write functions *only*. + * \brief 3D edge between two VertexSE3, uses the euler angle parameterization + * for the read/write functions *only*. */ - class EdgeSE3Euler : public EdgeSE3 +class EdgeSE3Euler : public EdgeSE3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/g2o/types/slam3d_addons/edge_se3_line.cpp b/g2o/types/slam3d_addons/edge_se3_line.cpp index 5cfa8209..5a15454c 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.cpp +++ b/g2o/types/slam3d_addons/edge_se3_line.cpp @@ -22,8 +22,8 @@ namespace Slam3dAddons { information().setZero(); for (int i=0; i<6; i++) for (int j=i; j<6; j++){ - is >> information()(i,j); - information()(i,j) = information()(j,i); + is >> information()(i,j); + information()(i,j) = information()(j,i); } information()(6,6) = 1e9; // normalization constraint return is.good(); @@ -39,7 +39,7 @@ namespace Slam3dAddons { } return os.good(); } - + void EdgeSE3Line3D::computeError() { const VertexLine3D* landmark=static_cast(vertices()[1]); Line3D projected(cache->w2n() * landmark->estimate()); @@ -55,15 +55,4 @@ namespace Slam3dAddons { return cache != 0; } - // void EdgeSE3Line3D::linearizeOplus() { - // return true; - // } - - // virtual bool EdgeSE3Line3D::setMeasurementFromState() { - // return true; - // } - - // virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to){ - // } - } diff --git a/g2o/types/slam3d_addons/edge_se3_line.h b/g2o/types/slam3d_addons/edge_se3_line.h index 657c7721..7895ce9c 100644 --- a/g2o/types/slam3d_addons/edge_se3_line.h +++ b/g2o/types/slam3d_addons/edge_se3_line.h @@ -46,48 +46,15 @@ namespace Slam3dAddons { return true; } - //void linearizeOplus(); - virtual int measurementDimension() const {return 7;} - /* - virtual bool setMeasurementFromState() ; - */ - /* virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /\*from*\/, */ - /* OptimizableGraph::Vertex* /\*to*\/) { */ - /* return 1.; */ - /* } */ - /* virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); */ - private: - + ParameterSE3Offset* offsetParam; CacheSE3Offset* cache; virtual bool resolveCaches(); }; -/* /\** */ -/* * \brief Output the pose-pose constraint to Gnuplot data file */ -/* *\/ */ -/* class G2O_TYPES_SLAM3D_API EdgeSE3WriteGnuplotAction: public WriteGnuplotAction { */ -/* public: */ -/* EdgeSE3WriteGnuplotAction(); */ -/* virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, */ -/* HyperGraphElementAction::Parameters* params_); */ -/* }; */ - -/* #ifdef G2O_HAVE_OPENGL */ -/* /\** */ -/* * \brief Visualize a 3D pose-pose constraint */ -/* *\/ */ -/* class G2O_TYPES_SLAM3D_API EdgeSE3DrawAction: public DrawAction{ */ -/* public: */ -/* EdgeSE3DrawAction(); */ -/* virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, */ -/* HyperGraphElementAction::Parameters* params_); */ -/* }; */ -/* #endif */ - } // end namespace #endif diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp index a59a986c..3703ed3f 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.cpp @@ -12,7 +12,7 @@ namespace Slam3dAddons { using namespace g2o; using namespace std; - + EdgeSE3PlaneSensorCalib::EdgeSE3PlaneSensorCalib() : BaseMultiEdge<3, Plane3D>() { @@ -80,7 +80,7 @@ namespace Slam3dAddons { return true; } - HyperGraphElementAction* EdgeSE3PlaneSensorCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element, + HyperGraphElementAction* EdgeSE3PlaneSensorCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) @@ -88,29 +88,26 @@ namespace Slam3dAddons { refreshPropertyPtrs(params_); - if (! _previousParams) + if (! _previousParams) return this; - + if (_show && !_show->value()) return this; EdgeSE3PlaneSensorCalib* that = dynamic_cast(element); - + if (! that) return this; const VertexSE3* robot = dynamic_cast(that->vertex(0)); const VertexSE3* sensor = dynamic_cast(that->vertex(2)); - cout << "that->vertex(0): " << that->vertex(0 -) << " that->vertex(2): " << that->vertex(2) << endl; - + //cout << "that->vertex(0): " << that->vertex(0) << " that->vertex(2): " << that->vertex(2) << endl; if (! robot|| ! sensor) return 0; - double d=that->measurement().distance(); double azimuth=Plane3D::azimuth(that->measurement().normal()); double elevation=Plane3D::elevation(that->measurement().normal()); @@ -118,18 +115,15 @@ namespace Slam3dAddons { // std::cerr << "azimuth=" << azimuth << std::endl; // std::cerr << "elevation=" << azimuth << std::endl; - glColor3f(that->color(0), that->color(1), that->color(2)); glPushMatrix(); Eigen::Isometry3d robotAndSensor = robot->estimate() * sensor->estimate(); glMultMatrixd(robotAndSensor.matrix().data()); - glRotatef(RAD2DEG(azimuth),0.,0.,1.); glRotatef(RAD2DEG(elevation),0.,-1.,0.); glTranslatef(d,0.,0.); - - + float planeWidth = 0.5; float planeHeight = 0.5; if (0) { @@ -147,7 +141,7 @@ namespace Slam3dAddons { } glPopMatrix(); - + return this; } #endif diff --git a/g2o/types/slam3d_addons/edge_se3_plane_calib.h b/g2o/types/slam3d_addons/edge_se3_plane_calib.h index 69aee076..87631c62 100644 --- a/g2o/types/slam3d_addons/edge_se3_plane_calib.h +++ b/g2o/types/slam3d_addons/edge_se3_plane_calib.h @@ -52,7 +52,7 @@ namespace Slam3dAddons { class EdgeSE3PlaneSensorCalibDrawAction: public DrawAction{ public: EdgeSE3PlaneSensorCalibDrawAction(); - virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, + virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_ ); protected: virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); diff --git a/g2o/types/slam3d_addons/line3d.cpp b/g2o/types/slam3d_addons/line3d.cpp index b9b53d14..3ddf200f 100644 --- a/g2o/types/slam3d_addons/line3d.cpp +++ b/g2o/types/slam3d_addons/line3d.cpp @@ -8,7 +8,7 @@ namespace Slam3dAddons { using namespace std; inline void _skew(Eigen::Matrix3d& S, const Eigen::Vector3d& t){ - S << + S << 0, -t.z(), t.y(), t.z(), 0, -t.x(), -t.y() ,t.x(), 0; @@ -16,7 +16,7 @@ namespace Slam3dAddons { inline Eigen::Matrix3d _skew(const Eigen::Vector3d& t){ Eigen::Matrix3d S; - S << + S << 0, -t.z(), t.y(), t.z(), 0, -t.x(), -t.y(), t.x(), 0; @@ -49,14 +49,14 @@ namespace Slam3dAddons { D.block<3,3>(0,3) = -2*_skew(l.w()); D.block<3,3>(3,3) = -2*_skew(l.d()); Jp.block<6,6>(0,0) = A*D; - + Vector3d d = l.d(); Vector3d w = l.w(); double ln = d.norm(); double iln = 1./ln; double iln3 = std::pow(iln,3); Matrix6d Jll; - Jll << + Jll << iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3, 0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3, 0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3, diff --git a/g2o/types/slam3d_addons/line3d.h b/g2o/types/slam3d_addons/line3d.h index b48eeb96..c7c07a7a 100644 --- a/g2o/types/slam3d_addons/line3d.h +++ b/g2o/types/slam3d_addons/line3d.h @@ -10,12 +10,12 @@ namespace Slam3dAddons { using namespace g2o; typedef Eigen::Matrix Matrix7x6d; - + typedef Eigen::Matrix Vector6d; - + typedef Eigen::Matrix Matrix6d; - - + + class Line3D : public Vector6d{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; @@ -29,7 +29,7 @@ namespace Slam3dAddons { } Vector6d toCartesian() const; - + inline Eigen::Vector3d w() const { return head<3>(); } @@ -69,7 +69,7 @@ namespace Slam3dAddons { *this+=v; normalize(); } - + inline Vector6d ominus(const Line3D& line){ return (Vector6d)(*this)-line; } @@ -77,13 +77,13 @@ namespace Slam3dAddons { static void jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Eigen::Isometry3d& x, const Line3D& l); }; - + Line3D operator*(const Eigen::Isometry3d& t, const Line3D& line); Vector6d transformCartesianLine(const Eigen::Isometry3d& t, const Vector6d& line); Vector6d normalizeCartesianLine(const Vector6d& line); - + } #endif diff --git a/g2o/types/slam3d_addons/line3d_test.cpp b/g2o/types/slam3d_addons/line3d_test.cpp index 41371462..8ee78294 100644 --- a/g2o/types/slam3d_addons/line3d_test.cpp +++ b/g2o/types/slam3d_addons/line3d_test.cpp @@ -9,16 +9,17 @@ using namespace g2o; using namespace g2o::internal; using namespace Slam3dAddons; -template -ostream& printVector(ostream& os, const T& t){ +template +ostream& printVector(ostream& os, const T& t) +{ for (int i=0; i