Commit 4ee32a4d authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Merge branch '19-building-a-new-visual-odometry-system' into 'devel'

Resolve "Building a new visual odometry system"

Closes #19

See merge request !28
parents 5ac8fc44 85af96d3
......@@ -31,5 +31,4 @@ src/CMakeFiles/cmake.check_cache
vision.found
\.vscode/
build_release/
doc/html
......@@ -81,7 +81,6 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ==================
FIND_PACKAGE(wolfcore REQUIRED)
FIND_PACKAGE(vision_utils REQUIRED)
SET(PRINT_INFO_VU false)
FIND_PACKAGE(OpenCV REQUIRED)
......@@ -108,7 +107,6 @@ include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ==================
INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
......@@ -117,16 +115,16 @@ SET(HDRS_CAPTURE
include/vision/capture/capture_image.h
)
SET(HDRS_FACTOR
include/vision/factor/factor_trifocal.h
include/vision/factor/factor_ahp.h
# include/vision/factor/factor_trifocal.h
# include/vision/factor/factor_ahp.h
include/vision/factor/factor_pixel_hp.h
include/vision/factor/factor_epipolar.h
# include/vision/factor/factor_epipolar.h
)
SET(HDRS_FEATURE
include/vision/feature/feature_point_image.h
)
SET(HDRS_LANDMARK
include/vision/landmark/landmark_ahp.h
# include/vision/landmark/landmark_ahp.h
include/vision/landmark/landmark_hp.h
include/vision/landmark/landmark_point_3d.h
)
......@@ -134,11 +132,8 @@ SET(HDRS_MATH
include/vision/math/pinhole_tools.h
)
SET(HDRS_PROCESSOR
include/vision/processor/processor_tracker_feature_trifocal.h
include/vision/processor/processor_params_image.h
include/vision/processor/processor_tracker_landmark_image.h
include/vision/processor/processor_tracker_feature_image.h
include/vision/processor/processor_bundle_adjustment.h
include/vision/processor/processor_visual_odometry.h
include/vision/processor/active_search.h
)
SET(HDRS_SENSOR
include/vision/sensor/sensor_camera.h
......@@ -152,25 +147,19 @@ SET(SRCS_FEATURE
src/feature/feature_point_image.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_ahp.cpp
# src/landmark/landmark_ahp.cpp
src/landmark/landmark_hp.cpp
src/landmark/landmark_point_3d.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature_trifocal.cpp
src/processor/processor_tracker_feature_image.cpp
src/processor/processor_tracker_landmark_image.cpp
src/processor/processor_bundle_adjustment.cpp
src/processor/processor_visual_odometry.cpp
src/processor/active_search.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
)
SET(SRCS_YAML
src/yaml/processor_tracker_feature_trifocal_yaml.cpp
src/yaml/sensor_camera_yaml.cpp
src/yaml/processor_image_yaml.cpp
src/yaml/processor_bundle_adjustment_yaml.cpp
)
)
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
......@@ -197,9 +186,8 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${OpenCV_LIBS})
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
#Build demos
#===============EXAMPLE=========================
......
<?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>
<?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>
......@@ -66,10 +66,6 @@ set(wolfvision_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Laser
FIND_PACKAGE(vision_utils REQUIRED)
list(APPEND wolfvision_INCLUDE_DIRS ${vision_utils_INCLUDE_DIR})
list(APPEND wolfvision_LIBRARIES ${vision_utils_LIBRARY})
FIND_PACKAGE(OpenCV REQUIRED)
list(APPEND wolfvision_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
list(APPEND wolfvision_LIBRARIES ${OpenCV_LIBS})
......
......@@ -12,7 +12,7 @@ else(OpenCV_FOUND)
message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
endif(OpenCV_FOUND)
ADD_EXECUTABLE(demo_processor_bundle_adjustment demo_processor_bundle_adjustment.cpp)
TARGET_LINK_LIBRARIES(demo_processor_bundle_adjustment ${PLUGIN_NAME} ${OpenCV_LIBS})
ADD_EXECUTABLE(demo_visual_odometry demo_visual_odometry.cpp)
TARGET_LINK_LIBRARIES(demo_visual_odometry ${PLUGIN_NAME} ${OpenCV_LIBS})
image_width: 640
image_height: 480
width: 752
height: 480
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [942.816482, 0.000000, 334.175643, 0.000000, 897.287666, 309.904004, 0.000000, 0.000000, 1.000000]
data: [458, 0.000000, 367.215,
0.000000, 457.296, 248.375,
0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.159958, -0.578834, 0.000000, 0.000000, 0.000000]
data: [-0.28340811, 0.07395907, 0.00019359, 0.0, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
data: [1.000000, 0.000000, 0.000000,
0.000000, 1.000000, 0.000000,
0.000000, 0.000000, 1.000000]
projection_matrix:
rows: 3
cols: 4
data: [953.235413, 0.000000, 334.175675, 0.000000, 0.000000, 907.202332, 309.904238, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
data: [355.63, 0.000000, 362.27, 0.000000,
0.000000, 417.16, 249.65, 0.000000,
0.000000, 0.000000, 1.000000, 0.000000]
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "core/math/pinhole_tools.h"
#include "core/landmark/landmark_AHP.h"
#include "core/factor/factor_AHP.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/sensor/sensor_camera.h"
#include "core/capture/capture_image.h"
int main()
{
using namespace wolf;
std::cout << std::endl << "==================== factor AHP test ======================" << std::endl;
TimeStamp t = 1;
//=====================================================
// Environment variable for configuration files
std::string wolf_root = _WOLF_ROOT_DIR;
std::cout << wolf_root << std::endl;
//=====================================================
// Wolf problem
ProblemPtr wolf_problem_ptr_ = Problem::create("PO", 3);
/* Do this while there aren't extrinsic parameters on the yaml */
Eigen::Vector7d extrinsic_cam;
extrinsic_cam.setRandom();
// extrinsic_cam[0] = 0; //px
// extrinsic_cam[1] = 0; //py
// extrinsic_cam[2] = 0; //pz
// extrinsic_cam[3] = 0; //qx
// extrinsic_cam[4] = 0; //qy
// extrinsic_cam[5] = 0; //qz
// extrinsic_cam[6] = 1; //qw
extrinsic_cam.tail<4>().normalize();
std::cout << "========extrinsic_cam: " << extrinsic_cam.transpose() << std::endl;
const Eigen::VectorXd extr = extrinsic_cam;
/* Do this while there aren't extrinsic parameters on the yaml */
SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("CAMERA", "PinHole", extr, wolf_root + "/src/examples/camera_params_ueye_radial_dist.yaml");
SensorCameraPtr camera_ptr_ = std::static_pointer_cast<SensorCamera>(sensor_ptr);
// PROCESSOR
// one-liner API
ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml");
// create the current frame
Eigen::Vector7d frame_pos_ori;
frame_pos_ori.setRandom();
// frame_pos_ori[0] = 0; //px
// frame_pos_ori[1] = 0; //py
// frame_pos_ori[2] = 0; //pz
// frame_pos_ori[3] = 0; //qx
// frame_pos_ori[4] = 0; //qy
// frame_pos_ori[5] = 0; //qz
// frame_pos_ori[6] = 1; //qw
frame_pos_ori.tail<4>().normalize();
const Eigen::VectorXd frame_val = frame_pos_ori;
FrameBasePtr last_frame = std::make_shared<FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
std::cout << "Last frame" << std::endl;
wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
// Capture
CaptureImagePtr image_ptr;
t.setToNow();
cv::Mat frame; //puede que necesite una imagen
image_ptr = std::make_shared< CaptureImage>(t, camera_ptr_, frame);
last_frame->addCapture(image_ptr);
// create the feature
cv::KeyPoint kp; kp.pt = {10,20};
cv::Mat desc;
FeaturePointImagePtr feat_point_image_ptr = std::make_shared<FeaturePointImage>(kp, 0, desc, Eigen::Matrix2d::Identity());
image_ptr->addFeature(feat_point_image_ptr);
FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
//FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectory()->getLastFrame();
// create the landmark
Eigen::Vector2d point2D;
point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
std::cout << "point2D: " << point2D.transpose() << std::endl;
double distance = 2; // arbitrary value
Eigen::Vector4d vec_homogeneous;
Eigen::VectorXd correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector();
std::cout << "correction vector: " << correction_vec << std::endl;
std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector() << std::endl;
point2D = pinhole::depixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D);
std::cout << "point2D depixellized: " << point2D.transpose() << std::endl;
point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector(),point2D);
std::cout << "point2D undistorted: " << point2D.transpose() << std::endl;
Eigen::Vector3d point3D;
point3D.head(2) = point2D;
point3D(2) = 1;
point3D.normalize();
std::cout << "point3D normalized: " << point3D.transpose() << std::endl;
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
std::cout << "vec_homogeneous: " << vec_homogeneous.transpose() << std::endl;
LandmarkAhpPtr landmark = std::make_shared<LandmarkAhp>(vec_homogeneous, anchor_frame, image_ptr->getSensor(), feat_point_image_ptr->getDescriptor());
std::cout << "Landmark AHP created" << std::endl;
// Create the factor
FactorAhpPtr factor_ptr = std::make_shared<FactorAhp>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAhp>(landmark), processor_ptr);
feat_point_image_ptr->addFactor(factor_ptr);
std::cout << "Factor AHP created" << std::endl;
Eigen::Vector2d point2D_proj = point3D.head<2>()/point3D(2);
std::cout << "point2D projected: " << point2D_proj.transpose() << std::endl;
Eigen::Vector2d point2D_dist;
point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector(),point2D_proj);
std::cout << "point2D distorted: " << point2D_dist.transpose() << std::endl;
Eigen::Vector2d point2D_pix;
point2D_pix = pinhole::pixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D_dist);
std::cout << "point2D pixellized: " << point2D_pix.transpose() << std::endl;
Eigen::Vector2d expectation;
Eigen::Vector3d current_frame_p = last_frame->getP()->getState();
Eigen::Vector4d current_frame_o = last_frame->getO()->getState();
Eigen::Vector3d anchor_frame_p = landmark->getAnchorFrame()->getP()->getState();
Eigen::Vector4d anchor_frame_o = landmark->getAnchorFrame()->getO()->getState();
Eigen::Vector4d landmark_ = landmark->getP()->getState();
factor_ptr ->expectation(current_frame_p.data(), current_frame_o.data(),
anchor_frame_p.data(), anchor_frame_o.data(),
landmark_.data(), expectation.data());
// current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual
std::cout << "expectation computed" << std::endl;
std::cout << "expectation = " << expectation[0] << " " << expectation[1] << std::endl;
return 0;
}
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
// Testing things for the 3D image odometry
//Wolf includes
#include "core/sensor/sensor_camera.h"
#include "core/capture/capture_image.h"
#include "core/processor/processor_tracker_feature_image.h"
#include "core/ceres_wrapper/solver_ceres.h"
// Vision utils includes
#include <vision_utils.h>
#include <sensors.h>
#include <common_class/buffer.h>
#include <common_class/frame.h>
//std includes
#include <ctime>
#include <iostream>
#include <string>
int main(int argc, char** argv)
{
using namespace wolf;