add plane prior

.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:           '^<ext/.*\.h>'
+    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
+...
+

include/g2o/edge_plane_prior.hpp

@@ -0,0 +1,50 @@
+#ifndef EDGE_PLANE_PRIOR_HPP
+#define EDGE_PLANE_PRIOR_HPP
+
+#include <Eigen/Dense>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+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<const g2o::VertexPlane*>(_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

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);

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<KernelData> kernels;

src/hdl_graph_slam/graph_slam.cpp

@@ -17,6 +17,7 @@
 #include <g2o/edge_se3_priorxyz.hpp>
 #include <g2o/edge_se3_priorvec.hpp>
 #include <g2o/edge_se3_priorquat.hpp>
+#include <g2o/edge_plane_prior.hpp>
 #include <g2o/edge_plane_parallel.hpp>
 #include <g2o/robust_kernel_io.hpp>
 
@@ -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);