Commit 17f76e73 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

visual odometry demo, inspired from apriltag demo, not tested

parent 6be621df
# wolf_demo_visual_odometry
## Getting started
To make it easy for you to get started with GitLab, here's a list of recommended next steps.
Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
## Add your files
- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
```
cd existing_repo
git remote add origin https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_visual_odometry.git
git branch -M main
git push -uf origin main
```
## Integrate with your tools
- [ ] [Set up project integrations](https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_visual_odometry/-/settings/integrations)
## Collaborate with your team
- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
- [ ] [Automatically merge when pipeline succeeds](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
## Test and Deploy
Use the built-in continuous integration in GitLab.
- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing(SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
***
# Editing this README
When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thank you to [makeareadme.com](https://www.makeareadme.com/) for this template.
## Suggestions for a good README
Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
## Name
Choose a self-explaining name for your project.
## Description
Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
## Badges
On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
## Visuals
Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
## Installation
Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
## Usage
Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
## Support
Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
## Roadmap
If you have ideas for releases in the future, it is a good idea to list them in the README.
## Contributing
State if you are open to contributions and what your requirements are for accepting them.
For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
## Authors and acknowledgment
Show your appreciation to those who have contributed to the project.
## License
For open source projects, say how it is licensed.
## Project status
If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
Check [WOLF documentation web](http://www.iri.upc.edu/wolf) (section WOLF Demos) for more information.
<!-- -->
<launch>
<!--USER ARGS-->
<arg name="rviz" default="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="sim_time" default="true" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
<!--VISUALIZATION-->
<group if="$(arg rviz)">
<node name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find wolf_demo_visual_odometry)/rviz/online.rviz" />
</group>
<!--WOLF-->
<node type="wolf_ros_node"
name="wolf_ros_node"
pkg="wolf_ros_node"
output="screen"
required="true"
launch-prefix="$(arg launch_pref)">
<param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/demo_visual_odometry_euroc.yaml" />
</node>
</launch>
<?xml version="1.0"?>
<package format="2">
<name>wolf_demo_visual_odometry</name>
<version>0.0.0</version>
<description>The wolf_demo_visual_odometry package</description>
<maintainer email="mfourmy@gmail.com">mfourmy</maintainer>
<license>BSD2</license>
<build_depend>wolf_ros_vision</build_depend>
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
- /Factors1/Namespaces1
- /Landmarks1/Namespaces1
- /Trajectory1/Namespaces1
Splitter Ratio: 0.5
Tree Height: 242
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Input image
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/Axes
Enabled: true
Length: 0.1
Name: Axes
Radius: 0.01
Show Trail: true
Reference Frame: base_footprint
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base_footprint:
Value: true
map:
Value: false
odom:
Value: false
Marker Alpha: 1
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
{}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_apriltag_vio/marker_array
Name: MarkerArray
Namespaces:
{}
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
factors_APRILTAG PROC: true
factors_IMU PROC: true
factors_text_APRILTAG PROC: false
factors_text_IMU PROC: false
factors_text_unnamed_processor: false
factors_unnamed_processor: true
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_landmarks
Name: Landmarks
Namespaces:
landmarks: true
landmarks_text: true
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
frames: true
frames_text: false
Queue Size: 1
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /camera_simu/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Input image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /image_debug
Max Value: 1
Median window: 5
Min Value: 0
Name: AprilTag Detections
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: visualization_marker
Name: Ground truth
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.5275864601135254
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.22070589661598206
Y: -0.007580682635307312
Z: -0.5612056255340576
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.7147972583770752
Target Frame: <Fixed Frame>
Yaw: 5.045364856719971
Saved: ~
Window Geometry:
AprilTag Detections:
collapsed: false
Displays:
collapsed: false
Height: 998
Hide Left Dock: false
Hide Right Dock: false
Input image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1267
X: 653
Y: 27
width: 752
height: 480
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
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.28340811, 0.07395907, 0.000000, 0.000000, 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]
projection_matrix:
rows: 3
cols: 4
data: [458, 0.000000, 367.215, 0.000000,
0.000000, 457.296, 248.375, 0.000000,
0.000000, 0.000000, 1.000000, 0.000000]
config:
debug:
profiling: true
profiling_file: "~/wolf_demo_visual_odometry_profiling.txt"
print_problem: false
print_depth: 2 # only if print_problem
print_state: false # only if print_problem
print_constr_by: false # only if print_problem
print_metric: false # only if print_problem
print_state_blocks: false # only if print_problem
print_period: 1 # only if print_problem
problem:
node_rate: 100
follow: "tree_manager.yaml"
frame_structure: "POV"
dimension: 3
prior:
mode: "factor"
$state:
P: [0,0,0]
O: [0,0,0,1] # x,y,z,w
V: [0,0,0]
$sigma:
P: [0.00001, 0.00001, 0.00001]
O: [0.01, 0.01, 0.01]
V: [0.1, 0.1, 0.1]
time_tolerance: 0.01
solver:
follow: "solver.yaml"
sensors:
-
type: "SensorCamera"
name: "CAMERA"
plugin: "vision"
extrinsic:
pose: [0,0,0, 0,0,0,1]
follow: "camera_euroc_mav0.yaml"
processors:
-
type: "ProcessorVisualOdometry"
name: "APRILTAG PROC"
sensor_name: "CAMERA"
plugin: "apriltag"
follow: "processor_visual_odometry.yaml"
ROS subscriber:
-
package: "wolf_ros_vision"
type: "SubscriberCamera"
topic: "/TODO"
sensor_name: "CAMERA"
ROS publisher:
-
type: "PublisherGraph"
topic: "graph"
package: "wolf_ros_node"
period: 0.1
viz_scale: 0.3
text_scale: 0.3
landmark_text_z_offset: 0.3
landmark_length: 0.5
frame_width: 0.01
frame_length: 0.1
frame_vel_scale: 1.0
-
type: "PublisherTf"
topic: " "
package: "wolf_ros_node"
period: 0.1
map_frame_id: "map"
odom_frame_id: "odom"
base_frame_id: "base_footprint"
publish_odom_tf: true
-
type: "PublisherVisionDebug"
topic: "/debug_image"
package: "wolf_ros_vision"
period: 0.1
algorithm:
time tolerance: 0.005
voting_active: true
klt_params:
tracker_width: 21
tracker_height: 21
nlevels_pyramids: 3
klt_max_err: 0.2
\ No newline at end of file
minimizer: "LEVENBERG_MARQUARDT"
interrupt_on_problem_change: false
min_num_iterations: 4
max_num_iterations: 20
function_tolerance: 1e-7
gradient_tolerance: 1e-9
use_nonmonotonic_steps: false
period: 0.3 # note: this must be smaller (i.e. faster) than the KF period in processor_tracker_landmark_apriltag.yaml
n_threads: 2
compute_cov: false
verbose: 0
\ No newline at end of file
tree_manager:
type: TreeManagerSlidingWindow
n_fix_first_frames: 1
n_frames: 15
viral_remove_empty_parent: true
# type: "none"
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment