diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..796f09f4 --- /dev/null +++ b/.clang-format @@ -0,0 +1,117 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Empty +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: false +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: false +ColumnLimit: 256 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^' + Priority: 2 + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Delimiter: pb + Language: TextProto + BasedOnStyle: google +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: false +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +... + diff --git a/include/g2o/edge_plane_prior.hpp b/include/g2o/edge_plane_prior.hpp new file mode 100644 index 00000000..afdd9e65 --- /dev/null +++ b/include/g2o/edge_plane_prior.hpp @@ -0,0 +1,50 @@ +#ifndef EDGE_PLANE_PRIOR_HPP +#define EDGE_PLANE_PRIOR_HPP + +#include +#include +#include + +namespace g2o { +class EdgePlanePriorNormal : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane> { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgePlanePriorNormal() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane>() {} + + void computeError() override { + const g2o::VertexPlane* v1 = static_cast(_vertices[0]); + Eigen::Vector3d normal = v1->estimate().normal(); + + if(normal.dot(_measurement) < 0.0) { + normal = -normal; + } + + _error = normal - _measurement; + } + + void setMeasurement(const Eigen::Vector3d& m) override { + _measurement = m; + } + + virtual bool read(std::istream& is) override { + Eigen::Vector3d v; + is >> v(0) >> v(1) >> v(2); + setMeasurement(v); + for(int i = 0; i < information().rows(); ++i) + for(int j = i; j < information().cols(); ++j) { + is >> information()(i, j); + if(i != j) information()(j, i) = information()(i, j); + } + return true; + } + virtual bool write(std::ostream& os) const override { + Eigen::Vector3d v = _measurement; + os << v(0) << " " << v(1) << " " << v(2) << " "; + for(int i = 0; i < information().rows(); ++i) + for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); + return os.good(); + } +}; +} // namespace g2o + +#endif // EDGE_SE3_PRIORXY_HPP diff --git a/include/hdl_graph_slam/graph_slam.hpp b/include/hdl_graph_slam/graph_slam.hpp index 648bbe99..dc5e2490 100644 --- a/include/hdl_graph_slam/graph_slam.hpp +++ b/include/hdl_graph_slam/graph_slam.hpp @@ -20,6 +20,7 @@ namespace g2o { class EdgePlane; class EdgePlaneParallel; class EdgePlanePerpendicular; + class EdgePlanePriorNormal; class RobustKernelFactory; } @@ -91,6 +92,8 @@ class GraphSLAM { * @param information_matrix * @return */ + g2o::EdgePlanePriorNormal* add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix); + g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); diff --git a/src/g2o/robust_kernel_io.cpp b/src/g2o/robust_kernel_io.cpp index 558045c1..15c6cb1c 100644 --- a/src/g2o/robust_kernel_io.cpp +++ b/src/g2o/robust_kernel_io.cpp @@ -94,8 +94,8 @@ class KernelData { bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { std::ifstream ifs(filename); if(!ifs) { - std::cerr << "failed to open input stream!!" << std::endl; - return false; + std::cerr << "warning: failed to open input stream!!" << std::endl; + return true; } std::vector kernels; diff --git a/src/hdl_graph_slam/graph_slam.cpp b/src/hdl_graph_slam/graph_slam.cpp index 93357ef6..61913c59 100644 --- a/src/hdl_graph_slam/graph_slam.cpp +++ b/src/hdl_graph_slam/graph_slam.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ namespace g2o { G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ) G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec) G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat) + G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_NORMAL, EdgePlanePriorNormal) G2O_REGISTER_TYPE(EDGE_PLANE_PARALLEL, EdgePlaneParallel) G2O_REGISTER_TYPE(EDGE_PLANE_PAERPENDICULAR, EdgePlanePerpendicular) } @@ -132,6 +134,16 @@ g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g return edge; } +g2o::EdgePlanePriorNormal* GraphSLAM::add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix) { + g2o::EdgePlanePriorNormal* edge(new g2o::EdgePlanePriorNormal()); + edge->setMeasurement(normal); + edge->setInformation(information_matrix); + edge->vertices()[0] = v; + graph->addEdge(edge); + + return edge; +} + g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY()); edge->setMeasurement(xy);