diff --git a/ROS-based code style eclipse indented 4sp.xml b/ROS-based code style eclipse indented 4sp.xml
deleted file mode 100644
index 73a372adf3fedd1fd54cf6889cfe0f570ef0e0c4..0000000000000000000000000000000000000000
--- a/ROS-based code style eclipse indented 4sp.xml
+++ /dev/null
@@ -1,167 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<profiles version="1">
-<profile kind="CodeFormatterProfile" name="ROS Formatting indented private" version="1">
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_in_empty_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.lineSplit" value="120"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_member_access" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_base_types" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_else_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_switch" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_constructor_initializer_list" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_base_types" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_access_specifier" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.use_tabs_only_for_leading_indentations" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_labeled_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_case" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.min_distance_between_code_and_line_comment" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_enum_declarations" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expressions_in_array_initializer" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_declarator_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_else_in_if_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_enumerator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_declarator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_empty_lines" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_empty_array_initializer_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_method_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.put_empty_statement_on_new_line" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_braces_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_method_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_breaks_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.join_wrapped_lines" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_declarator_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_arguments_in_method_invocation" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.never_indent_line_comments_on_first_column" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_while" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_brackets" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_parameters_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.number_of_empty_lines_to_preserve" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.preserve_white_space_between_code_and_line_comments" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_type_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_expression_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expression_list" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_default" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.format_guardian_clause_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_extra_spaces" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_compare_to_type_header" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_type_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation_for_array_initializer" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_labeled_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_semicolon_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_namespace_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_compact_if" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_at_end_of_file_if_missing" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_assignment" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression_chain" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_expression_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_exception_specification" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_base_clause_in_type_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_identifier_in_function_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_throws" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_invocation_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_declaration_compare_to_template_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_body" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_throws" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_binary_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_block" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_catch_in_try_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_throws_clause_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.char" value="space"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_colon_in_constructor_initializer_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_invocation_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block_in_case" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.compact_else_if" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_template_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_then_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_switch" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_overloaded_left_shift_chain" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_if" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_imple_if_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indentation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_namespace_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_enum_declarations" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_array_initializer" value="end_of_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_case" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_catch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_namespace_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_while_in_do_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-</profile>
-</profiles>
diff --git a/ROS-based wolf code style QT indented 4sp.xml b/ROS-based wolf code style QT indented 4sp.xml
deleted file mode 100644
index 7ef65eaad10d85b649675af569f07ed7f463792a..0000000000000000000000000000000000000000
--- a/ROS-based wolf code style QT indented 4sp.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorCodeStyle>
-<!-- Written by QtCreator 3.0.1, 2016-02-22T12:02:02. -->
-<qtcreator>
- <data>
- <variable>CodeStyleData</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="AlignAssignments">false</value>
- <value type="bool" key="AutoSpacesForTabs">false</value>
- <value type="bool" key="BindStarToIdentifier">false</value>
- <value type="bool" key="BindStarToLeftSpecifier">false</value>
- <value type="bool" key="BindStarToRightSpecifier">false</value>
- <value type="bool" key="BindStarToTypeName">true</value>
- <value type="bool" key="ExtraPaddingForConditionsIfConfusingAlign">true</value>
- <value type="bool" key="IndentAccessSpecifiers">true</value>
- <value type="bool" key="IndentBlockBody">true</value>
- <value type="bool" key="IndentBlockBraces">false</value>
- <value type="bool" key="IndentBlocksRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentClassBraces">false</value>
- <value type="bool" key="IndentControlFlowRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentDeclarationsRelativeToAccessSpecifiers">true</value>
- <value type="bool" key="IndentEnumBraces">false</value>
- <value type="bool" key="IndentFunctionBody">true</value>
- <value type="bool" key="IndentFunctionBraces">false</value>
- <value type="bool" key="IndentNamespaceBody">false</value>
- <value type="bool" key="IndentNamespaceBraces">false</value>
- <value type="int" key="IndentSize">4</value>
- <value type="bool" key="IndentStatementsRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentSwitchLabels">true</value>
- <value type="int" key="PaddingMode">2</value>
- <value type="bool" key="SpacesForTabs">true</value>
- <value type="int" key="TabSize">4</value>
- </valuemap>
- </data>
- <data>
- <variable>DisplayName</variable>
- <value type="QString">ROS wolf</value>
- </data>
-</qtcreator>
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
deleted file mode 100644
index 85f43146f9e51b952d6bc33e75bcda962262dd0a..0000000000000000000000000000000000000000
--- a/codetemplates eclipse.xml
+++ /dev/null
@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?><templates><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorcomment_context" deleted="false" description="Comment for created constructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorcomment" name="constructorcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorcomment_context" deleted="false" description="Comment for created destructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorcomment" name="destructorcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.ui.text.codetemplates.filecomment_context" deleted="false" description="Comment for created C/C++ files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.filecomment" name="filecomment">/**
- * \file ${file_name}
- *
- * Created on: ${date}
- * \author: ${user}
- */
-</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.typecomment_context" deleted="false" description="Comment for created classes" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.typecomment" name="typecomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.fieldcomment_context" deleted="false" description="Comment for fields" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.fieldcomment" name="fieldcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodcomment_context" deleted="false" description="Comment for methods" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodcomment" name="methodcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppsourcefile" name="Default C++ source template">${filecomment}
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
-
-#include <core/utils/utils_gtest.h>
-
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-TEST(TestGroup, DummyTestExample)
-{
- // ${todo}: Automatically generated TEST stub
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-${namespace_end}</template><template autoinsert="true" context="org.eclipse.cdt.core.cxxHeader.contenttype_context" deleted="false" description="Default template for newly created C++ header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppheaderfile" name="Default C++ header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-${namespace_end}
-
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.core.cSource.contenttype_context" deleted="false" description="Default template for newly created C source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.csourcefile" name="Default C source template">${filecomment}
-${includes}
-
-${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.core.cHeader.contenttype_context" deleted="false" description="Default template for newly created C header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cheaderfile" name="Default C header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-
-${includes}
-
-${declarations}
-
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="Beginning of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_begin" name="namespace_begin">namespace ${namespace_name} {</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="End of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_end" name="namespace_end">} /* namespace ${namespace_name} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.class_context" deleted="false" description="Code in created class definitions" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.class_body" name="class_body">${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodbody_context" deleted="false" description="Code in created method stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodbody" name="methodbody"> // ${todo} Auto-generated method stub
- ${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorbody_context" deleted="false" description="Code in created constructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorbody" name="constructorbody"> // ${todo} Auto-generated constructor stub
- ${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorbody_context" deleted="false" description="Code in created destructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorbody" name="destructorbody"> ${body_statement}
- // ${todo} Auto-generated destructor stub</template><template autoinsert="true" context="org.eclipse.cdt.core.asmSource.contenttype_context" deleted="false" description="Default template for newly created assembly files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.asmsourcefile" name="Default assembly template">${filecomment}
-</template><template autoinsert="true" context="org.eclipse.core.runtime.text.contenttype_context" deleted="false" description="Default template for newly created text files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.textfile" name="Default text file template">${file_name}
- Created on: ${date}
- Author: ${user}
-
-</template></templates>
\ No newline at end of file
diff --git a/include/imu/factor/factor_compass_3d.h b/include/imu/factor/factor_compass_3d.h
index c58af6e1ae13b5744d24612a900a8986a95d0bf1..2c4738723a961e8383fb830c4e7ca8ea8b5f6b03 100644
--- a/include/imu/factor/factor_compass_3d.h
+++ b/include/imu/factor/factor_compass_3d.h
@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef FACTOR_COMPASS_3D_H_
-#define FACTOR_COMPASS_3D_H_
+#pragma once
#include <core/factor/factor_autodiff.h>
@@ -31,53 +30,61 @@ WOLF_PTR_TYPEDEFS(FactorCompass3d);
class FactorCompass3d : public FactorAutodiff<FactorCompass3d, 3, 4, 4, 3, 3>
{
- public:
- FactorCompass3d(const FeatureBasePtr& _feat,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff("FactorCompass3d",
- TOP_ABS,
- _feat,
- nullptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getO(),
- _feat->getCapture()->getSensor()->getO(),
- _feat->getCapture()->getSensor()->getIntrinsic(),
- _feat->getCapture()->getSensor()->getStateBlock('F'))
- {
- }
+ public:
+ FactorCompass3d(const FeatureBasePtr &_feat,
+ const ProcessorBasePtr &_processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE)
+ : FactorAutodiff("FactorCompass3d",
+ TOP_ABS,
+ _feat->getMeasurement(),
+ _feat->getMeasurementSquareRootInformationUpper(),
+ (_feat->getCapture()->getSensor()->isStateBlockDynamic('O') and
+ _feat->getCapture()->getSensor()->isStateBlockDynamic('I') and
+ _feat->getCapture()->getSensor()->isStateBlockDynamic('F'))
+ ? NodeStateBlocksPtrList{_feat->getFrame(), _feat->getCapture()} // all states dynamic
+ : (not _feat->getCapture()->getSensor()->isStateBlockDynamic('O') and
+ not _feat->getCapture()->getSensor()->isStateBlockDynamic('I') and
+ not _feat->getCapture()->getSensor()->isStateBlockDynamic('F'))
+ ? NodeStateBlocksPtrList{_feat->getFrame(), _feat->getCapture()->getSensor()} // all states static
+ : NodeStateBlocksPtrList{_feat->getFrame(),
+ _feat->getCapture(),
+ _feat->getCapture()->getSensor()}, // some dynamic, some static
+ _processor_ptr,
+ {_feat->getFrame()->getO(),
+ _feat->getCapture()->getSensor()->getO(),
+ _feat->getCapture()->getSensor()->getIntrinsic(),
+ _feat->getCapture()->getSensor()->getStateBlockDynamic('F')},
+ _apply_loss_function,
+ _status)
+ {
+ }
- ~FactorCompass3d() override { /* nothing */ }
+ ~FactorCompass3d() override
+ { /* nothing */
+ }
- template<typename T>
- bool operator () (const T* const _frm_q,
- const T* const _sen_q,
- const T* const _sen_bias,
- const T* const _field,
- T* _residual) const
- {
- using namespace Eigen;
+ template <typename T>
+ bool operator()(const T *const _frm_q,
+ const T *const _sen_q,
+ const T *const _sen_bias,
+ const T *const _field,
+ T *_residual) const
+ {
+ using namespace Eigen;
- Map<const Quaternion<T>> frm_q(_frm_q);
- Map<const Quaternion<T>> sen_q(_sen_q);
- Map<const Matrix<T,3,1>> sen_bias(_sen_bias);
- Map<const Matrix<T,3,1>> field(_field);
- Map<Matrix<T,3,1>> res(_residual);
+ Map<const Quaternion<T>> frm_q(_frm_q);
+ Map<const Quaternion<T>> sen_q(_sen_q);
+ Map<const Matrix<T, 3, 1>> sen_bias(_sen_bias);
+ Map<const Matrix<T, 3, 1>> field(_field);
+ Map<Matrix<T, 3, 1>> res(_residual);
- Matrix<T,3,1> exp = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
+ Matrix<T, 3, 1> exp = sen_q.conjugate() * frm_q.conjugate() * field + sen_bias;
- res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - exp);
+ res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - exp);
- return true;
- }
+ return true;
+ }
};
-} /* namespace wolf */
-
-#endif /* FACTOR_COMPASS_3D_H_ */
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/imu/factor/factor_fix_bias.h b/include/imu/factor/factor_fix_bias.h
index dc6faf4c96fd2b1fa2c99f1cfa751b92f42f9e0a..0583029e26839638c63f22727a0df95525c646c5 100644
--- a/include/imu/factor/factor_fix_bias.h
+++ b/include/imu/factor/factor_fix_bias.h
@@ -23,90 +23,92 @@
#ifndef FACTOR_FIX_BIAS_H_
#define FACTOR_FIX_BIAS_H_
-//Wolf includes
+// Wolf includes
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
#include <core/factor/factor_autodiff.h>
#include <core/frame/frame_base.h>
#include <core/math/rotations.h>
-//#include "ceres/jet.h"
+// #include "ceres/jet.h"
+
+namespace wolf
+{
-namespace wolf {
-
WOLF_PTR_TYPEDEFS(FactorFixBias);
-//class
-class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
+// class
+class FactorFixBias : public FactorAutodiff<FactorFixBias, 6, 3, 3>
{
- public:
- FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorFixBias, 6, 3, 3>("FactorFixBias",
- TOP_ABS,
- _ftr_ptr,
- nullptr, nullptr, nullptr, nullptr,
- nullptr,
- _apply_loss_function,
- _status,
- std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getAccBias(),
- std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getGyroBias())
- {
- // std::cout << "created FactorFixBias " << std::endl;
- }
-
- virtual ~FactorFixBias() = default;
-
- template<typename T>
- bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
-
+ public:
+ FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE)
+ : FactorAutodiff<FactorFixBias, 6, 3, 3>(
+ "FactorFixBias",
+ TOP_ABS,
+ _ftr_ptr->getMeasurement(),
+ _ftr_ptr->getMeasurementSquareRootInformationUpper(),
+ {_ftr_ptr->getCapture()},
+ {std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getAccBias(),
+ std::static_pointer_cast<CaptureImu>(_ftr_ptr->getCapture())->getGyroBias()},
+ _apply_loss_function,
+ _status)
+ {
+ // std::cout << "created FactorFixBias " << std::endl;
+ }
+
+ virtual ~FactorFixBias() = default;
+
+ template <typename T>
+ bool operator()(const T* const _ab, const T* const _wb, T* _residuals) const;
};
-template<typename T>
-inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
+template <typename T>
+inline bool FactorFixBias::operator()(const T* const _ab, const T* const _wb, T* _residuals) const
{
// measurement
- Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
- Eigen::Matrix<T,3,1> ab(_ab);
- Eigen::Matrix<T,3,1> wb(_wb);
+ Eigen::Matrix<T, 6, 1> meas = getMeasurement().cast<T>();
+ Eigen::Matrix<T, 3, 1> ab(_ab);
+ Eigen::Matrix<T, 3, 1> wb(_wb);
// error
- Eigen::Matrix<T,6,1> er;
+ Eigen::Matrix<T, 6, 1> er;
er.head(3) = meas.head(3) - ab;
er.tail(3) = meas.tail(3) - wb;
// residual
- Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
+ Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-// using ceres::Jet;
-// Eigen::MatrixXd J(6,6);
-// J.row(0) = ((Jet<double, 3>)(er(0))).v;
-// J.row(1) = ((Jet<double, 3>)(er(1))).v;
-// J.row(2) = ((Jet<double, 3>)(er(2))).v;
-// J.row(3) = ((Jet<double, 3>)(er(3))).v;
-// J.row(4) = ((Jet<double, 3>)(er(4))).v;
-// J.row(5) = ((Jet<double, 3>)(er(5))).v;
-
-// J.row(0) = ((Jet<double, 3>)(res(0))).v;
-// J.row(1) = ((Jet<double, 3>)(res(1))).v;
-// J.row(2) = ((Jet<double, 3>)(res(2))).v;
-// J.row(3) = ((Jet<double, 3>)(res(3))).v;
-// J.row(4) = ((Jet<double, 3>)(res(4))).v;
-// J.row(5) = ((Jet<double, 3>)(res(5))).v;
-
-// if (sizeof(er(0)) != sizeof(double))
-// {
-// std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
+ // using ceres::Jet;
+ // Eigen::MatrixXd J(6,6);
+ // J.row(0) = ((Jet<double, 3>)(er(0))).v;
+ // J.row(1) = ((Jet<double, 3>)(er(1))).v;
+ // J.row(2) = ((Jet<double, 3>)(er(2))).v;
+ // J.row(3) = ((Jet<double, 3>)(er(3))).v;
+ // J.row(4) = ((Jet<double, 3>)(er(4))).v;
+ // J.row(5) = ((Jet<double, 3>)(er(5))).v;
+
+ // J.row(0) = ((Jet<double, 3>)(res(0))).v;
+ // J.row(1) = ((Jet<double, 3>)(res(1))).v;
+ // J.row(2) = ((Jet<double, 3>)(res(2))).v;
+ // J.row(3) = ((Jet<double, 3>)(res(3))).v;
+ // J.row(4) = ((Jet<double, 3>)(res(4))).v;
+ // J.row(5) = ((Jet<double, 3>)(res(5))).v;
+
+ // if (sizeof(er(0)) != sizeof(double))
+ // {
+ // std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
+ // std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+ // std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() <<
+ // std::endl;
+ // }
////////////////////////////////////////////////////////
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 7ea62c9608a60e2a17ff07a9c7c93a8ceb914f58..5049eebe0baeaa8e8e4bb1e1668967b4b69dad70 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -22,130 +22,130 @@
#ifndef FACTOR_IMU_THETA_H_
#define FACTOR_IMU_THETA_H_
-//Wolf includes
+// Wolf includes
#include "imu/feature/feature_imu.h"
#include "imu/sensor/sensor_imu.h"
#include <core/factor/factor_autodiff.h>
#include <core/math/rotations.h>
-//Eigen include
+// Eigen include
+
+namespace wolf
+{
-namespace wolf {
-
WOLF_PTR_TYPEDEFS(FactorImu);
-//class
+// class
class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
{
- public:
- FactorImu(const FeatureImuPtr& _ftr_ptr,
- const CaptureImuPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorImu() override = default;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _o2,
- const T* const _v2,
- T* _res) const;
-
- /** \brief Estimation error given the states in the wolf tree
- *
- */
- Eigen::Vector9d error();
-
-
- /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- * params :
- * Vector3d _p1 : position in imu frame
- * Vector4d _q1 : orientation quaternion in imu frame
- * Vector3d _v1 : velocity in imu frame
- * Vector3d _ab : accelerometer bias in imu frame
- * Vector3d _wb : gyroscope bias in imu frame
- * Vector3d _p2 : position in current frame
- * Vector4d _q2 : orientation quaternion in current frame
- * Vector3d _v2 : velocity in current frame
- * Matrix<9,1, double> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
- */
- template<typename D1, typename D2, typename D3>
- bool residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- Eigen::MatrixBase<D3> & _res) const;
-
- /** Function expectation(...)
- * params :
- * Vector3d _p1 : position in imu frame
- * Vector4d _q1 : orientation quaternion in imu frame
- * Vector3d _v1 : velocity in imu frame
- * Vector3d _p2 : position in current frame
- * Vector4d _q2 : orientation quaternion in current frame
- * Vector3d _v2 : velocity in current frame
- * double _dt : time interval between the two states
- * _pe : expected position delta
- * _qe : expected quaternion delta
- * _ve : expected velocity delta
- */
- template<typename D1, typename D2, typename D3, typename D4>
- void expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const;
-
- /** \brief : return the expected delta given the state blocks in the wolf tree
- */
- Eigen::VectorXd expectation() const;
-
- private:
- /// Preintegrated delta
- Eigen::Vector3d dp_preint_;
- Eigen::Quaterniond dq_preint_;
- Eigen::Vector3d dv_preint_;
-
- // Biases used during preintegration
- Eigen::Vector3d acc_bias_preint_;
- Eigen::Vector3d gyro_bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix3d dDp_dab_;
- Eigen::Matrix3d dDv_dab_;
- Eigen::Matrix3d dDp_dwb_;
- Eigen::Matrix3d dDv_dwb_;
- Eigen::Matrix3d dDq_dwb_;
-
- /// Metrics
- const double dt_; ///< delta-time and delta-time-squared between keyframes
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ public:
+ FactorImu(const FeatureImuPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorImu() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ */
+ template <typename T>
+ bool operator()(const T* const _p1,
+ const T* const _o1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _v2,
+ T* _res) const;
+
+ /** \brief Estimation error given the states in the wolf tree
+ *
+ */
+ Eigen::Vector9d error();
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ * params :
+ * Vector3d _p1 : position in imu frame
+ * Vector4d _q1 : orientation quaternion in imu frame
+ * Vector3d _v1 : velocity in imu frame
+ * Vector3d _ab : accelerometer bias in imu frame
+ * Vector3d _wb : gyroscope bias in imu frame
+ * Vector3d _p2 : position in current frame
+ * Vector4d _q2 : orientation quaternion in current frame
+ * Vector3d _v2 : velocity in current frame
+ * Matrix<9,1, double> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
+ */
+ template <typename D1, typename D2, typename D3>
+ bool residual(const Eigen::MatrixBase<D1>& _p1,
+ const Eigen::QuaternionBase<D2>& _q1,
+ const Eigen::MatrixBase<D1>& _v1,
+ const Eigen::MatrixBase<D1>& _ab1,
+ const Eigen::MatrixBase<D1>& _wb1,
+ const Eigen::MatrixBase<D1>& _p2,
+ const Eigen::QuaternionBase<D2>& _q2,
+ const Eigen::MatrixBase<D1>& _v2,
+ Eigen::MatrixBase<D3>& _res) const;
+
+ /** Function expectation(...)
+ * params :
+ * Vector3d _p1 : position in imu frame
+ * Vector4d _q1 : orientation quaternion in imu frame
+ * Vector3d _v1 : velocity in imu frame
+ * Vector3d _p2 : position in current frame
+ * Vector4d _q2 : orientation quaternion in current frame
+ * Vector3d _v2 : velocity in current frame
+ * double _dt : time interval between the two states
+ * _pe : expected position delta
+ * _qe : expected quaternion delta
+ * _ve : expected velocity delta
+ */
+ template <typename D1, typename D2, typename D3, typename D4>
+ void expectation(const Eigen::MatrixBase<D1>& _p1,
+ const Eigen::QuaternionBase<D2>& _q1,
+ const Eigen::MatrixBase<D1>& _v1,
+ const Eigen::MatrixBase<D1>& _p2,
+ const Eigen::QuaternionBase<D2>& _q2,
+ const Eigen::MatrixBase<D1>& _v2,
+ typename D1::Scalar _dt,
+ Eigen::MatrixBase<D3>& _pe,
+ Eigen::QuaternionBase<D4>& _qe,
+ Eigen::MatrixBase<D3>& _ve) const;
+
+ /** \brief : return the expected delta given the state blocks in the wolf tree
+ */
+ Eigen::VectorXd expectation() const;
+
+ private:
+ /// Preintegrated delta
+ Eigen::Vector3d dp_preint_;
+ Eigen::Quaterniond dq_preint_;
+ Eigen::Vector3d dv_preint_;
+
+ // Biases used during preintegration
+ Eigen::Vector3d acc_bias_preint_;
+ Eigen::Vector3d gyro_bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Eigen::Matrix3d dDp_dab_;
+ Eigen::Matrix3d dDv_dab_;
+ Eigen::Matrix3d dDp_dwb_;
+ Eigen::Matrix3d dDv_dwb_;
+ Eigen::Matrix3d dDq_dwb_;
+
+ /// Metrics
+ const double dt_; ///< delta-time and delta-time-squared between keyframes
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
///////////////////// IMPLEMENTAITON ////////////////////////////
@@ -154,64 +154,66 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
const CaptureImuPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
- "FactorImu",
- TOP_MOTION,
- _ftr_ptr,
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getP(),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_origin_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV()),
- dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
- dq_preint_(_ftr_ptr->getMeasurement().data()+3),
- dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
- acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
- gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
- dDp_dab_(_ftr_ptr->getJacobianBias().block(0,0,3,3)), // Jacs of dp dv dq wrt biases
- dDv_dab_(_ftr_ptr->getJacobianBias().block(6,0,3,3)),
- dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
- dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
- dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
+ FactorStatus _status)
+ : FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
+ "FactorImu",
+ TOP_MOTION,
+ _ftr_ptr->getMeasurement(),
+ _ftr_ptr->getMeasurementSquareRootInformationUpper(),
+ NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(),
+ _ftr_ptr->getFrame(),
+ (_cap_origin_ptr->getSensor()->isStateBlockDynamic('I')
+ ? _cap_origin_ptr->shared_from_this()
+ : _cap_origin_ptr->getSensor()->shared_from_this())},
+ _processor_ptr,
+ {_cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV()},
+ _apply_loss_function,
+ _status),
+ dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
+ dq_preint_(_ftr_ptr->getMeasurement().data() + 3),
+ dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
+ acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
+ gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
+ dDp_dab_(_ftr_ptr->getJacobianBias().block(0, 0, 3, 3)), // Jacs of dp dv dq wrt biases
+ dDv_dab_(_ftr_ptr->getJacobianBias().block(6, 0, 3, 3)),
+ dDp_dwb_(_ftr_ptr->getJacobianBias().block(0, 3, 3, 3)),
+ dDv_dwb_(_ftr_ptr->getJacobianBias().block(6, 3, 3, 3)),
+ dDq_dwb_(_ftr_ptr->getJacobianBias().block(3, 3, 3, 3)),
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
{
//
}
-template<typename T>
-inline bool FactorImu::operator ()(const T* const _p1,
- const T* const _q1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _q2,
- const T* const _v2,
- T* _res) const
+template <typename T>
+inline bool FactorImu::operator()(const T* const _p1,
+ const T* const _q1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _q2,
+ const T* const _v2,
+ T* _res) const
{
using namespace Eigen;
// MAPS
- Map<const Matrix<T,3,1> > p1(_p1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T,3,1> > v1(_v1);
- Map<const Matrix<T,3,1> > ab1(_b1);
- Map<const Matrix<T,3,1> > wb1(_b1 + 3);
+ Map<const Matrix<T, 3, 1> > p1(_p1);
+ Map<const Quaternion<T> > q1(_q1);
+ Map<const Matrix<T, 3, 1> > v1(_v1);
+ Map<const Matrix<T, 3, 1> > ab1(_b1);
+ Map<const Matrix<T, 3, 1> > wb1(_b1 + 3);
- Map<const Matrix<T,3,1> > p2(_p2);
- Map<const Quaternion<T> > q2(_q2);
- Map<const Matrix<T,3,1> > v2(_v2);
+ Map<const Matrix<T, 3, 1> > p2(_p2);
+ Map<const Quaternion<T> > q2(_q2);
+ Map<const Matrix<T, 3, 1> > v2(_v2);
- Map<Matrix<T,9,1> > res(_res);
+ Map<Matrix<T, 9, 1> > res(_res);
residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
@@ -222,8 +224,8 @@ Eigen::Vector9d FactorImu::error()
{
Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
- Map<const Vector3d > acc_bias(bias.data());
- Map<const Vector3d > gyro_bias(bias.data() + 3);
+ Map<const Vector3d> acc_bias(bias.data());
+ Map<const Vector3d> gyro_bias(bias.data() + 3);
Eigen::Vector10d delta_exp = expectation();
@@ -231,9 +233,9 @@ Eigen::Vector9d FactorImu::error()
Eigen::Vector9d delta_step;
- delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
- delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
- delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
+ delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
+ delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
+ delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
@@ -242,18 +244,17 @@ Eigen::Vector9d FactorImu::error()
return err;
}
-template<typename D1, typename D2, typename D3>
-inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- Eigen::MatrixBase<D3> & _res) const
+template <typename D1, typename D2, typename D3>
+inline bool FactorImu::residual(const Eigen::MatrixBase<D1>& _p1,
+ const Eigen::QuaternionBase<D2>& _q1,
+ const Eigen::MatrixBase<D1>& _v1,
+ const Eigen::MatrixBase<D1>& _ab1,
+ const Eigen::MatrixBase<D1>& _wb1,
+ const Eigen::MatrixBase<D1>& _p2,
+ const Eigen::QuaternionBase<D2>& _q2,
+ const Eigen::MatrixBase<D1>& _v2,
+ Eigen::MatrixBase<D3>& _res) const
{
-
/* Help for the Imu residual function
*
* Notations:
@@ -274,9 +275,8 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
* T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
*
* Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
- * T_err = diff(D_exp, D_corr) // compare against expected delta
- * res = W.sqrt * T_err
+ * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias
+ * change T_err = diff(D_exp, D_corr) // compare against expected delta res = W.sqrt * T_err
*
* results in:
* res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
@@ -291,17 +291,17 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
*
* NOTE: See optimization report at the end of this file for comparisons of both methods.
*/
-#define METHOD_1 // if commented, then METHOD_2 will be applied
+#define METHOD_1 // if commented, then METHOD_2 will be applied
- //needed typedefs
+ // needed typedefs
typedef typename D1::Scalar T;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 9);
// 1. Expected delta from states
- Eigen::Matrix<T, 3, 1 > dp_exp;
- Eigen::Quaternion<T> dq_exp;
- Eigen::Matrix<T, 3, 1> dv_exp;
+ Eigen::Matrix<T, 3, 1> dp_exp;
+ Eigen::Quaternion<T> dq_exp;
+ Eigen::Matrix<T, 3, 1> dv_exp;
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, dt_, dp_exp, dq_exp, dv_exp);
@@ -310,68 +310,80 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
Eigen::Matrix<T, 9, 1> d_step;
- d_step.block(0,0,3,1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
- d_step.block(3,0,3,1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_);
- d_step.block(6,0,3,1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(0, 0, 3, 1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(3, 0, 3, 1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(6, 0, 3, 1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
-#ifdef METHOD_1 // method 1
- Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
- Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1);
- Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1);
+#ifdef METHOD_1 // method 1
+ Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0, 0, 3, 1);
+ Eigen::Matrix<T, 3, 1> do_step = d_step.block(3, 0, 3, 1);
+ Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6, 0, 3, 1);
// 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
- Eigen::Matrix<T,3,1> dp_correct;
- Eigen::Quaternion<T> dq_correct;
- Eigen::Matrix<T,3,1> dv_correct;
-
- imu::plus(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_step, do_step, dv_step,
- dp_correct, dq_correct, dv_correct);
+ Eigen::Matrix<T, 3, 1> dp_correct;
+ Eigen::Quaternion<T> dq_correct;
+ Eigen::Matrix<T, 3, 1> dv_correct;
+
+ imu::plus(dp_preint_.cast<T>(),
+ dq_preint_.cast<T>(),
+ dv_preint_.cast<T>(),
+ dp_step,
+ do_step,
+ dv_step,
+ dp_correct,
+ dq_correct,
+ dv_correct);
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
- Eigen::Matrix<T, 9, 1> d_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
+ Eigen::Matrix<T, 9, 1> d_error;
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data());
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
Eigen::Matrix<T, 3, 3> J_do_dq1, J_do_dq2;
Eigen::Matrix<T, 9, 9> J_err_corr;
-//#define WITH_JAC
+// #define WITH_JAC
#ifdef WITH_JAC
- imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error, J_do_dq1, J_do_dq2);
+ imu::diff(
+ dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error, J_do_dq1, J_do_dq2);
J_err_corr.setIdentity();
- J_err_corr.block(3,3,3,3) = J_do_dq2;
+ J_err_corr.block(3, 3, 3, 3) = J_do_dq2;
// 4. Residuals are the weighted errors
- _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper() * d_error;
+ _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper() * d_error;
#else
imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
_res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
-#else // method 2
+#else // method 2
- Eigen::Matrix<T, 9, 1> d_diff;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
+ Eigen::Matrix<T, 9, 1> d_diff;
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data());
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
- imu::diff(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_exp, dq_exp, dv_exp,
- dp_diff, do_diff, dv_diff);
+ imu::diff(dp_preint_.cast<T>(),
+ dq_preint_.cast<T>(),
+ dv_preint_.cast<T>(),
+ dp_exp,
+ dq_exp,
+ dv_exp,
+ dp_diff,
+ do_diff,
+ dv_diff);
Eigen::Matrix<T, 9, 1> d_error;
d_error << d_diff - d_step;
// 4. Residuals are the weighted errors
- _res = getMeasurementSquareRootInformationUpper() * d_error;
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
-
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
#if 0
@@ -437,49 +449,55 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
inline Eigen::VectorXd FactorImu::expectation() const
{
- auto frm_current = getFeature()->getFrame();
+ auto frm_current = getFeature()->getFrame();
auto frm_previous = getFrameOther();
- //get information on current_frame in the FactorImu
- const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
- const Eigen::Quaterniond frame_current_ori (frm_current->getO()->getState().data());
- const Eigen::Vector3d frame_current_vel = (frm_current->getV()->getState());
+ // get information on current_frame in the FactorImu
+ const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
+ const Eigen::Quaterniond frame_current_ori(frm_current->getO()->getState().data());
+ const Eigen::Vector3d frame_current_vel = (frm_current->getV()->getState());
// get info on previous frame in the FactorImu
- const Eigen::Vector3d frame_previous_pos = (frm_previous->getP()->getState());
- const Eigen::Quaterniond frame_previous_ori (frm_previous->getO()->getState().data());
- const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
+ const Eigen::Vector3d frame_previous_pos = (frm_previous->getP()->getState());
+ const Eigen::Quaterniond frame_previous_ori(frm_previous->getO()->getState().data());
+ const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
// Define results vector and Map bits over it
- Eigen::Matrix<double, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<double, 3, 1> > pe(exp.data() );
- Eigen::Map<Eigen::Quaternion<double> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<double, 3, 1> > ve(exp.data() + 7);
-
- expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
- frame_current_pos, frame_current_ori, frame_current_vel,
+ Eigen::Matrix<double, 10, 1> exp;
+ Eigen::Map<Eigen::Matrix<double, 3, 1> > pe(exp.data());
+ Eigen::Map<Eigen::Quaternion<double> > qe(exp.data() + 3);
+ Eigen::Map<Eigen::Matrix<double, 3, 1> > ve(exp.data() + 7);
+
+ expectation(frame_previous_pos,
+ frame_previous_ori,
+ frame_previous_vel,
+ frame_current_pos,
+ frame_current_ori,
+ frame_current_vel,
dt_,
- pe, qe, ve);
+ pe,
+ qe,
+ ve);
return exp;
}
-template<typename D1, typename D2, typename D3, typename D4>
-inline void FactorImu::expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const
+template <typename D1, typename D2, typename D3, typename D4>
+inline void FactorImu::expectation(const Eigen::MatrixBase<D1>& _p1,
+ const Eigen::QuaternionBase<D2>& _q1,
+ const Eigen::MatrixBase<D1>& _v1,
+ const Eigen::MatrixBase<D1>& _p2,
+ const Eigen::QuaternionBase<D2>& _q2,
+ const Eigen::MatrixBase<D1>& _v2,
+ typename D1::Scalar _dt,
+ Eigen::MatrixBase<D3>& _pe,
+ Eigen::QuaternionBase<D4>& _qe,
+ Eigen::MatrixBase<D3>& _ve) const
{
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
}
-} // namespace wolf
+} // namespace wolf
#endif
@@ -496,47 +514,49 @@ inline void FactorImu::expectation(const Eigen::MatrixBase<D1> & _p1,
* With Method 1:
*
[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
-[trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][10:58:16] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][10:58:16] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
+[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final
+cost: 1.251480e-06, Termination: CONVERGENCE [trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower
+than 1.57 approx) = 0.5 1 1.5 [ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][10:58:16] [gtest_Imu.cpp L564 : TestBody] Ceres Solver
+Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][10:58:16] [gtest_Imu.cpp L638 : TestBody] Ceres Solver
+Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
[----------] 3 tests from Process_Factor_Imu (159 ms total)
[----------] 2 tests from Process_Factor_Imu_ODO
[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][10:58:16] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
+[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03,
+Final cost: 1.867678e-22, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms) [ RUN ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][10:58:16] [gtest_Imu.cpp L783 : TestBody] Ceres
+Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
[----------] 2 tests from Process_Factor_Imu_ODO (120 ms total)
*
* With Method 2:
*
[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
-[trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][11:15:43] [gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][11:15:43] [gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
+[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final
+cost: 1.251479e-06, Termination: CONVERGENCE [trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower
+than 1.57 approx) = 0.5 1 1.5 [ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][11:15:43] [gtest_Imu.cpp L564 : TestBody] Ceres Solver
+Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][11:15:43] [gtest_Imu.cpp L638 : TestBody] Ceres Solver
+Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
[----------] 3 tests from Process_Factor_Imu (133 ms total)
[----------] 2 tests from Process_Factor_Imu_ODO
[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][11:15:43] [gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
+[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03,
+Final cost: 1.855814e-22, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms) [ RUN ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][11:15:43] [gtest_Imu.cpp L783 : TestBody] Ceres
+Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
[----------] 2 tests from Process_Factor_Imu_ODO (127 ms total)
*
*/
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
index 42fe2fa292f4b71a5c031bbf68e1f3744ed872d8..6a95c6b236c32ba9538c4e06c5510fa7b561e3aa 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu2d.h
@@ -22,157 +22,158 @@
#ifndef FACTOR_IMU2D_THETA_H_
#define FACTOR_IMU2D_THETA_H_
-//Wolf includes
+// Wolf includes
#include "imu/feature/feature_imu2d.h"
#include "imu/math/imu2d_tools.h"
#include <core/factor/factor_autodiff.h>
#include <core/math/rotations.h>
-//Eigen include
+// Eigen include
+
+namespace wolf
+{
-namespace wolf {
-
WOLF_PTR_TYPEDEFS(FactorImu2d);
-//class
+// class
class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
{
- public:
- FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
- const CaptureImuPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorImu2d() override = default;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _o2,
- const T* const _v2,
- T* _res) const;
-
- Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
- {
- WOLF_WARN_COND(!_ftr_ptr->getCapture()->getSensor(), "FactorImu2d: null Sensor Pointer");
-
- if (not _ftr_ptr or not _ftr_ptr->getCapture() or not _ftr_ptr->getCapture()->getSensor())
- return Eigen::Matrix3d::Identity();
-
-
- return _ftr_ptr->getCapture()->getSensor()->getDriftStd('I').cwiseInverse().asDiagonal();
- }
-
- private:
- /// Preintegrated delta
- Eigen::Vector5d delta_preint_;
-
- // Biases used during preintegration
- Eigen::Vector3d bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix<double, 5, 3> jacobian_bias_;
-
- /// Metrics
- const double dt_; ///< delta-time and delta-time-squared between keyframes
-
- const Eigen::Matrix5d sqrt_delta_preint_inv_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ public:
+ FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorImu2d() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ */
+ template <typename T>
+ bool operator()(const T* const _p1,
+ const T* const _o1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _v2,
+ T* _res) const;
+
+ Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
+ {
+ WOLF_WARN_COND(!_ftr_ptr->getCapture()->getSensor(), "FactorImu2d: null Sensor Pointer");
+
+ if (not _ftr_ptr or not _ftr_ptr->getCapture() or not _ftr_ptr->getCapture()->getSensor())
+ return Eigen::Matrix3d::Identity();
+
+ return _ftr_ptr->getCapture()->getSensor()->getDriftStd('I').cwiseInverse().asDiagonal();
+ }
+
+ private:
+ /// Preintegrated delta
+ Eigen::Vector5d delta_preint_;
+
+ // Biases used during preintegration
+ Eigen::Vector3d bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Eigen::Matrix<double, 5, 3> jacobian_bias_;
+
+ /// Metrics
+ const double dt_; ///< delta-time and delta-time-squared between keyframes
+
+ const Eigen::Matrix5d sqrt_delta_preint_inv_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
+inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
const CaptureImuPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>( // ...
- "FactorImu2d",
- TOP_MOTION,
- _ftr_ptr,
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getP(),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_origin_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV()),
- delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
- bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
- jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
+ FactorStatus _status)
+ : FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>( // ...
+ "FactorImu2d",
+ TOP_MOTION,
+ _ftr_ptr->getMeasurement(),
+ _ftr_ptr->getMeasurementSquareRootInformationUpper(),
+ {_cap_origin_ptr->getFrame(),
+ _ftr_ptr->getFrame(),
+ _cap_origin_ptr->getSensor()->isStateBlockDynamic('I') ? _cap_origin_ptr->shared_from_this()
+ : _cap_origin_ptr->getSensor()->shared_from_this()},
+ _processor_ptr,
+ {_cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV()},
+ _apply_loss_function,
+ _status),
+ delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
+ bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
+ jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
-template<typename T>
-inline bool FactorImu2d::operator ()(const T* const _p1,
- const T* const _th1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _th2,
- const T* const _v2,
- T* _res) const
+template <typename T>
+inline bool FactorImu2d::operator()(const T* const _p1,
+ const T* const _th1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _th2,
+ const T* const _v2,
+ T* _res) const
{
using namespace Eigen;
// MAPS
- Map<const Matrix<T,2,1> > p1(_p1);
- const T& th1 = *_th1;
- Map<const Matrix<T,2,1> > v1(_v1);
- Map<const Matrix<T,3,1> > b1(_b1);
+ Map<const Matrix<T, 2, 1> > p1(_p1);
+ const T& th1 = *_th1;
+ Map<const Matrix<T, 2, 1> > v1(_v1);
+ Map<const Matrix<T, 3, 1> > b1(_b1);
- Map<const Matrix<T,2,1> > p2(_p2);
- const T& th2 = *_th2;
- Map<const Matrix<T,2,1> > v2(_v2);
+ Map<const Matrix<T, 2, 1> > p2(_p2);
+ const T& th2 = *_th2;
+ Map<const Matrix<T, 2, 1> > v2(_v2);
- Map<Matrix<T,5,1> > res(_res);
+ Map<Matrix<T, 5, 1> > res(_res);
- //residual
+ // residual
/*
* MATH:
- * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 (-) x1))
+ * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 (-) x1))
*/
- Matrix<T, 5, 1> delta_step = jacobian_bias_*(b1 - bias_preint_);
+ Matrix<T, 5, 1> delta_step = jacobian_bias_ * (b1 - bias_preint_);
Matrix<T, 5, 1> delta_preint = delta_preint_.cast<T>();
- Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step);
+ Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step);
- Matrix<T, 5, 1> delta_predicted;
- Map<Matrix<T,2,1> > dp_predicted( &delta_predicted(0) + 0);
- T& dth_predicted = delta_predicted(2);
- Map<Matrix<T,2,1> > dv_predicted(&delta_predicted(0) + 3);
- imu2d::betweenStates(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted);
+ Matrix<T, 5, 1> delta_predicted;
+ Map<Matrix<T, 2, 1> > dp_predicted(&delta_predicted(0) + 0);
+ T& dth_predicted = delta_predicted(2);
+ Map<Matrix<T, 2, 1> > dv_predicted(&delta_predicted(0) + 3);
+ imu2d::betweenStates(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted);
Matrix<T, 5, 1> delta_error = delta_corr - delta_predicted;
- delta_error(2) = pi2pi(delta_error(2));
+ delta_error(2) = pi2pi(delta_error(2));
res = sqrt_delta_preint_inv_ * delta_error;
-
+
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif
diff --git a/include/imu/factor/factor_imu2d_with_gravity.h b/include/imu/factor/factor_imu2d_with_gravity.h
index 9f022bce04a053b716bb54d7f7028222ab88ae07..e89891654b23d71a3afbcef939cc66aa827867b3 100644
--- a/include/imu/factor/factor_imu2d_with_gravity.h
+++ b/include/imu/factor/factor_imu2d_with_gravity.h
@@ -22,159 +22,170 @@
#ifndef FACTOR_IMU2D_WITH_GRAVITY_H_
#define FACTOR_IMU2D_WITH_GRAVITY_H_
-//Wolf includes
+// Wolf includes
#include "imu/feature/feature_imu2d.h"
#include "imu/math/imu2d_tools.h"
#include <core/factor/factor_autodiff.h>
#include <core/math/rotations.h>
+namespace wolf
+{
-namespace wolf {
-
WOLF_PTR_TYPEDEFS(FactorImu2dWithGravity);
-//class
+// class
class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>
{
- public:
- FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
- const CaptureImuPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorImu2dWithGravity() override = default;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _o2,
- const T* const _v2,
- const T* const _g,
- T* _res) const;
- Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
- {
- WOLF_WARN_COND(not _ftr_ptr->getCapture()->getSensor(), "FactorImu2d: null Sensor Pointer");
-
- if (not _ftr_ptr or not _ftr_ptr->getCapture() or not _ftr_ptr->getCapture()->getSensor())
- return Eigen::Matrix3d::Identity();
-
-
- return _ftr_ptr->getCapture()->getSensor()->getDriftStd('I').cwiseInverse().asDiagonal();
- }
-
- private:
- /// Preintegrated delta
- Eigen::Vector5d delta_preint_;
-
- // Biases used during preintegration
- Eigen::Vector3d bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix<double, 5, 3> jacobian_bias_;
-
- /// Metrics
- const double dt_; ///< delta-time and delta-time-squared between keyframes
-
- const Eigen::Matrix5d sqrt_delta_preint_inv_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ public:
+ FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorImu2dWithGravity() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ */
+ template <typename T>
+ bool operator()(const T* const _p1,
+ const T* const _o1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _v2,
+ const T* const _g,
+ T* _res) const;
+ Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
+ {
+ WOLF_WARN_COND(not _ftr_ptr->getCapture()->getSensor(), "FactorImu2d: null Sensor Pointer");
+
+ if (not _ftr_ptr or not _ftr_ptr->getCapture() or not _ftr_ptr->getCapture()->getSensor())
+ return Eigen::Matrix3d::Identity();
+
+ return _ftr_ptr->getCapture()->getSensor()->getDriftStd('I').cwiseInverse().asDiagonal();
+ }
+
+ private:
+ /// Preintegrated delta
+ Eigen::Vector5d delta_preint_;
+
+ // Biases used during preintegration
+ Eigen::Vector3d bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Eigen::Matrix<double, 5, 3> jacobian_bias_;
+
+ /// Metrics
+ const double dt_; ///< delta-time and delta-time-squared between keyframes
+
+ const Eigen::Matrix5d sqrt_delta_preint_inv_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
- const CaptureImuPtr& _cap_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>( // ...
- "FactorImu2dWithGravity",
- TOP_MOTION,
- _ftr_ptr,
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getP(),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_origin_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensor()->getStateBlock('G')),
- delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
- bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
- jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
+inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>( // ...
+ "FactorImu2dWithGravity",
+ TOP_MOTION,
+ _ftr_ptr->getMeasurement(),
+ _ftr_ptr->getMeasurementSquareRootInformationUpper(),
+ _cap_origin_ptr->getSensor()->isStateBlockDynamic('I') and
+ _cap_origin_ptr->getSensor()->isStateBlockDynamic('F')
+ // b&g dynamic
+ ? NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(), _ftr_ptr->getFrame(), _cap_origin_ptr}
+ : (not _cap_origin_ptr->getSensor()->isStateBlockDynamic('I') and
+ not _cap_origin_ptr->getSensor()->isStateBlockDynamic('F')
+ // b&g static
+ ? NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(),
+ _ftr_ptr->getFrame(),
+ _cap_origin_ptr->getSensor()} //
+ // one dynamic other static
+ : NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(),
+ _ftr_ptr->getFrame(),
+ _cap_origin_ptr,
+ _cap_origin_ptr->getSensor()}),
+ _processor_ptr,
+ {_cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensor()->getStateBlockDynamic('G')},
+ _apply_loss_function,
+ _status),
+ delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
+ bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
+ jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
-template<typename T>
-inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
- const T* const _th1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _th2,
- const T* const _v2,
- const T* const _g,
- T* _res) const
+template <typename T>
+inline bool FactorImu2dWithGravity::operator()(const T* const _p1,
+ const T* const _th1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _th2,
+ const T* const _v2,
+ const T* const _g,
+ T* _res) const
{
using namespace Eigen;
// MAPS
- Map<const Matrix<T,2,1> > p1(_p1);
- const T& th1 = *_th1;
- Map<const Matrix<T,2,1> > v1(_v1);
- Map<const Matrix<T,3,1> > b1(_b1);
+ Map<const Matrix<T, 2, 1> > p1(_p1);
+ const T& th1 = *_th1;
+ Map<const Matrix<T, 2, 1> > v1(_v1);
+ Map<const Matrix<T, 3, 1> > b1(_b1);
- Map<const Matrix<T,2,1> > p2(_p2);
- const T& th2 = *_th2;
- Map<const Matrix<T,2,1> > v2(_v2);
- Map<const Matrix<T,2,1> > g(_g);
+ Map<const Matrix<T, 2, 1> > p2(_p2);
+ const T& th2 = *_th2;
+ Map<const Matrix<T, 2, 1> > v2(_v2);
+ Map<const Matrix<T, 2, 1> > g(_g);
- Map<Matrix<T,5,1> > res(_res);
+ Map<Matrix<T, 5, 1> > res(_res);
- //residual
+ // residual
/*
* MATH:
- * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 <-> x1))
+ * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 <-> x1))
*/
- Matrix<T, 5, 1> delta_step = jacobian_bias_*(b1 - bias_preint_);
+ Matrix<T, 5, 1> delta_step = jacobian_bias_ * (b1 - bias_preint_);
Matrix<T, 5, 1> delta_preint = delta_preint_.cast<T>();
- Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step);
+ Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step);
- Matrix<T, 5, 1> delta_predicted;
- Map<Matrix<T,2,1> > dp_predicted( &delta_predicted(0) + 0);
- T& dth_predicted = delta_predicted(2);
- Map<Matrix<T,2,1> > dv_predicted(&delta_predicted(0) + 3);
- imu2d::betweenStatesWithGravity(p1, th1, v1, p2, th2, v2, dt_, g, dp_predicted, dth_predicted, dv_predicted);
+ Matrix<T, 5, 1> delta_predicted;
+ Map<Matrix<T, 2, 1> > dp_predicted(&delta_predicted(0) + 0);
+ T& dth_predicted = delta_predicted(2);
+ Map<Matrix<T, 2, 1> > dv_predicted(&delta_predicted(0) + 3);
+ imu2d::betweenStatesWithGravity(p1, th1, v1, p2, th2, v2, dt_, g, dp_predicted, dth_predicted, dv_predicted);
Matrix<T, 5, 1> delta_error = imu2d::diff(delta_predicted, delta_corr);
- delta_error(2) = pi2pi(delta_error(2));
+ delta_error(2) = pi2pi(delta_error(2));
+
+ res = sqrt_delta_preint_inv_ * delta_error;
- res = sqrt_delta_preint_inv_*delta_error;
-
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index 672646233f580b1c925904528f1eff442bcbf058..d881bca0cce2063adc680597a975dbc8c15f9dcc 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -19,21 +19,14 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * imu2d_tools.h
- *
- * Created on: Nov 16, 2020
- * Author: igeer
- */
-#ifndef IMU2D_TOOLS_H_
-#define IMU2D_TOOLS_H_
+#pragma once
#include <core/common/wolf.h>
#include <core/math/rotations.h>
#include <core/math/SE2.h>
-#include <core/state_block/vector_composite.h>
-#include <core/state_block/state_composite.h>
+#include <core/composite/vector_composite.h>
+#include <core/composite/matrix_composite.h>
#include <cstdio>
@@ -979,6 +972,4 @@ inline void body2delta(const MatrixBase<D1>& body,
//}
} // namespace imu2d
-} // namespace wolf
-
-#endif /* Imu_TOOLS_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 60a0a00f30715a5601ffb8822e475fb1e4553a0e..b3f54691f9a994e4b209748289394566acc169ae 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -19,20 +19,13 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * imu_tools.h
- *
- * Created on: Jul 29, 2017
- * Author: jsola
- */
-#ifndef IMU_TOOLS_H_
-#define IMU_TOOLS_H_
+#pragma once
#include <core/common/wolf.h>
#include <core/math/rotations.h>
-#include <core/state_block/vector_composite.h>
-#include <core/state_block/state_composite.h>
+#include <core/composite/vector_composite.h>
+#include <core/composite/matrix_composite.h>
#include <cstdio>
@@ -929,6 +922,4 @@ void extract_v0_g(const MatrixBase<TP0> &p0,
}
} // namespace imu
-} // namespace wolf
-
-#endif /* Imu_TOOLS_H_ */
+} // namespace wolf
\ No newline at end of file
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 07f06456f291ff014b2466aed1c8230086f92621..6541c9fde23316f068d9385263f777f4fbc6d432 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -51,7 +51,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion
bootstrap_enable = _n["bootstrap"]["enable"].as<bool>();
if (bootstrap_enable)
{
- string str = _n["bootstrap"]["method"].as<string>();
+ std::string str = _n["bootstrap"]["method"].as<std::string>();
std::transform(str.begin(), str.end(), str.begin(), ::toupper);
if (str == "STATIC" /**/)
{
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index 8f961524bc937486ded115ac80598b1bb8c00723..85b8af424661d705110f7810a05a2c006875a104 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -58,7 +58,7 @@ class SensorCompass : public SensorBase
public:
SensorCompass(ParamsSensorCompassPtr _params,
- const SpecSensorComposite& _priors);
+ const SpecStateSensorComposite& _priors);
WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass);
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 7b207bfc9c5be67b9013b557472871c89de8b0ab..8f956da686349344f61ade1a4c62b7a26b7e2324 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -67,7 +67,7 @@ class SensorImu : public SensorBase
public:
SensorImu(ParamsSensorImuPtr _params,
- const SpecSensorComposite& _priors) :
+ const SpecStateSensorComposite& _priors) :
SensorBase("SensorImu",
DIM,
_params,
@@ -76,7 +76,7 @@ class SensorImu : public SensorBase
{
};
- WOLF_SENSOR_CREATE(SensorImu<DIM>, ParamsSensorImu);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM, ParamsSensorImu);
double getGyroNoise() const;
double getAccelNoise() const;
diff --git a/package.xml b/package.xml
deleted file mode 100644
index e94a5f327b94328c07c868e760a402a5f3cf14cc..0000000000000000000000000000000000000000
--- a/package.xml
+++ /dev/null
@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
- <name>wolf</name>
- <version>0.0.1</version>
- <description>Windowed Localization Frames</description>
-
- <maintainer email="jsola@iri.upc.edu">Joan Sola</maintainer>
- <maintainer email="jvallve@iri.upc.edu">Joan Vallvé Navarro</maintainer>
- <maintainer email="todo@todo.com">todo</maintainer>
-
- <author>Joan Sola</author>
- <author>Joan Vallvé Navarro</author>
- <author>todo</author>
-
- <license>todo</license>
-
- <buildtool_depend>cmake</buildtool_depend>
-
- <build_depend>eigen</build_depend>
-
- <!-- wolf does not REQUIRE the following, they are added here for the dependency tree -->
- <build_depend>Ceres</build_depend>
- <build_depend>laser_scan_utils</build_depend>
- <build_depend>raw_gps_utils</build_depend>
- <build_depend>OpenCV</build_depend>
- <build_depend>YAMLCPP</build_depend>
-
- <export>
- <build_type>cmake</build_type>
- </export>
-
-</package>
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 27559fb4c62cec4f80ba2543170c7038c6ef0d34..ba500c0bb7ae7024846a751efa8b780674275478 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -25,7 +25,7 @@
#include "imu/math/imu_tools.h"
// wolf
-#include <core/state_block/vector_composite.h>
+#include <core/composite/vector_composite.h>
#include <core/factor/factor_block_difference.h>
namespace wolf {
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 466e6f294320518edf2057ba01597eebdfb1a849..a48cb0e1ba0f1d6992a16831789ee1987cb429d6 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -28,7 +28,7 @@
#include "imu/math/imu2d_tools.h"
// wolf
-#include <core/state_block/vector_composite.h>
+#include <core/composite/vector_composite.h>
#include <core/factor/factor_block_difference.h>
namespace wolf {
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index d0416bf6a3e5d920b923b0be0ee1a23e8fa464a1..2f9d4cd6c6afba6a7434aef92a9142cbc53fc56e 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -27,7 +27,7 @@ namespace wolf
{
SensorCompass::SensorCompass(ParamsSensorCompassPtr _params,
- const SpecSensorComposite& _priors) :
+ const SpecStateSensorComposite& _priors) :
SensorBase("SensorCompass",
3,
_params,