Patch/rviz plugin

[ruffsl]

Revert prior work around in favor of enabling the use_rgbd_sensor_base_tf_functor parameter.

[ruffsl]

There seems to be an issue with enabling the use_rgbd_sensor_base_tf_functor parameter:

``` $ ros2 launch omnimapper_ros realsense.launch.xml use_rosbag:=false [INFO] [launch]: All log files can be found below /home/ruffsl/.ros/log/2020-04-01-20-20-51-192640-cog-rw07-5681 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [omnimapper_ros_node-1]: process started with pid [5692] [INFO] [odom_to_base_link-2]: process started with pid [5693] [INFO] [base_link_to_camera_link-3]: process started with pid [5694] [omnimapper_ros_node-1] BoundedPlanePlugin: Constructor. [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: OmniMapperROS: Constructing... Loading ROS Params... [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: Waiting for initial pose from odom to base_link [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: GetTransformFunctorTF: waiting for camera_color_optical_frame to base_link to be ready: First attempt. [omnimapper_ros_node-1] [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: OmniMapperROS: Installing BoundedPlanePlugin callback. [omnimapper_ros_node-1] BoundedPlanePlugin: Got 2 regions. [omnimapper_ros_node-1] border before: 2779 [omnimapper_ros_node-1] border after: 2560 [omnimapper_ros_node-1] BoundedPlanePlugin: GOT VALID MEASUREMENT! [omnimapper_ros_node-1] Model: -0.303592 -0.891558 0.336089 -1.272950, Base Model: -0.336089 -0.303592 -0.891558 1.272950 [omnimapper_ros_node-1] border before: 2867 [omnimapper_ros_node-1] border after: 2383 [omnimapper_ros_node-1] BoundedPlanePlugin: GOT VALID MEASUREMENT! [omnimapper_ros_node-1] Model: 0.949506 -0.238007 0.204427 -0.751833, Base Model: -0.204427 0.949506 -0.238007 0.751833 [omnimapper_ros_node-1] BoundedPlanePlugin: Have 2 measurements [omnimapper_ros_node-1] BoundedPlanePlugin: Processing planes for pose 8646911284551352320 [omnimapper_ros_node-1] BoundedPlanePlugin: Creating new plane 7061644215716937728: -0.336089 -0.303592 -0.891558 1.272950 [omnimapper_ros_node-1] Adding new symbol: b0 [omnimapper_ros_node-1] BoundedPlanePlugin: Added factor! [omnimapper_ros_node-1] ERROR: Initializing boundary at bad place: Point is 0.014080 from plane. ```

Looks like the legacy code base has a few magic numbers:

https://github.com/CogRob/omnimapper/blob/da7bddaf47b6ec9aceb1c99efe926f8253d6a2b2/src/plugins/bounded_plane_plugin.cpp#L237-L242

Doubling such thresholds only seems to result in further run time errors with other magic numbers:

``` $ ros2 launch omnimapper_ros realsense.launch.xml use_rosbag:=false [INFO] [launch]: All log files can be found below /home/ruffsl/.ros/log/2020-04-01-20-27-27-789948-cog-rw07-7214 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [omnimapper_ros_node-1]: process started with pid [7227] [INFO] [odom_to_base_link-2]: process started with pid [7228] [INFO] [base_link_to_camera_link-3]: process started with pid [7229] [omnimapper_ros_node-1] BoundedPlanePlugin: Constructor. [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: OmniMapperROS: Constructing... Loading ROS Params... [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: Waiting for initial pose from odom to base_link [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist [omnimapper_ros_node-1] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: GetTransformFunctorTF: waiting for camera_color_optical_frame to base_link to be ready: First attempt. [omnimapper_ros_node-1] [omnimapper_ros_node-1] [INFO] [omnimapper_ros_node]: OmniMapperROS: Installing BoundedPlanePlugin callback. [omnimapper_ros_node-1] BoundedPlanePlugin: Got 2 regions. [omnimapper_ros_node-1] border before: 1386 [omnimapper_ros_node-1] border after: 1233 [omnimapper_ros_node-1] BoundedPlanePlugin: GOT VALID MEASUREMENT! [omnimapper_ros_node-1] Model: 0.966958 -0.025305 -0.253678 0.257125, Base Model: -0.253678 -0.966958 0.025305 0.271350 [omnimapper_ros_node-1] border before: 1304 [omnimapper_ros_node-1] border after: 1248 [omnimapper_ros_node-1] BoundedPlanePlugin: GOT VALID MEASUREMENT! [omnimapper_ros_node-1] Model: -0.962848 0.013001 0.269731 -0.267086, Base Model: -0.269731 -0.962848 0.013001 0.267086 [omnimapper_ros_node-1] BoundedPlanePlugin: Have 2 measurements [omnimapper_ros_node-1] BoundedPlanePlugin: Processing planes for pose 8646911284551352320 [omnimapper_ros_node-1] BoundedPlanePlugin: map_meas: GOT INVALID MEASUREMENT! [omnimapper_ros_node-1] BoundedPlanePlugin: Creating new plane 7061644215716937728: -0.253678 -0.966958 0.025305 0.271350 [omnimapper_ros_node-1] Adding new symbol: b0 [omnimapper_ros_node-1] BoundedPlane3: retracting: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: transform translation: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] -0 [omnimapper_ros_node-1] BoundedPlanePlugin: Added factor! [omnimapper_ros_node-1] BoundedPlane3: error: -0 [omnimapper_ros_node-1] 0 0 [omnimapper_ros_node-1] BoundedPlaneFactor: error: -0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlanePlugin: map_meas: GOT INVALID MEASUREMENT! [omnimapper_ros_node-1] BoundedPlanePlugin: Creating new plane 7061644215716937729: -0.269731 -0.962848 0.013001 0.267086 [omnimapper_ros_node-1] BoundedPlane3: error: -0 [omnimapper_ros_node-1] 0 0 [omnimapper_ros_node-1] BoundedPlaneFactor: error: -0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] RViz Plugin: Cloud had 1233 points [omnimapper_ros_node-1] RViz Plugin Centroid: 0.419046 0.169923 -0.029184 [omnimapper_ros_node-1] BoundedPlane3: retracting: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: transform translation: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] -0 [omnimapper_ros_node-1] Adding new symbol: b1 [omnimapper_ros_node-1] BoundedPlanePlugin: Added factor! [omnimapper_ros_node-1] BoundedPlane3: error: -6.44407e-18 [omnimapper_ros_node-1] -4.94626e-19 0 [omnimapper_ros_node-1] BoundedPlaneFactor: error: -6.44407e-18 [omnimapper_ros_node-1] -4.94626e-19 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: error: -0 [omnimapper_ros_node-1] 0 0 [omnimapper_ros_node-1] BoundedPlaneFactor: error: -0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: error: -6.44407e-18 [omnimapper_ros_node-1] -4.94626e-19 0 [omnimapper_ros_node-1] BoundedPlaneFactor: error: -6.44407e-18 [omnimapper_ros_node-1] -4.94626e-19 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] RViz Plugin: Cloud had 1233 points [omnimapper_ros_node-1] RViz Plugin Centroid: 0.419046 0.169923 -0.029184 [omnimapper_ros_node-1] BoundedPlane3: retracting: -2.4649e-38 [omnimapper_ros_node-1] 7.45156e-40 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: transform translation: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] -0 [omnimapper_ros_node-1] BoundedPlane3: retracting: 6.44407e-18 [omnimapper_ros_node-1] 4.94626e-19 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] BoundedPlane3: transform translation: 0 [omnimapper_ros_node-1] 0 [omnimapper_ros_node-1] -0 [omnimapper_ros_node-1] ERROR: Retract fail: Point is 0.014160 from plane. ```

https://github.com/CogRob/omnimapper/blob/da7bddaf47b6ec9aceb1c99efe926f8253d6a2b2/src/BoundedPlane3.cpp#L61-L63

Not sure of what was intended by only exiting hard when a magic threshold is exceed. @atrevor ?

[BillWSY]

Consider changing this https://github.com/CogRob/omnimapper/blob/da7bddaf47b6ec9aceb1c99efe926f8253d6a2b2/src/plugins/bounded_plane_plugin.cpp#L142-L148 to this: https://github.com/CogRob/omnimapper/blob/84e9173c7ca1b151902516812edab2f1d26a5f5b/src/bounded_plane_plugin.cpp#L131-L133

[BillWSY]

I was wondering if Alex can verify my patch?

I thought the code was trying to make sure the normal vector of the plane is pointing towards the same direction as the vector from origin to a point on the plane (i.e. they form an acute angle). In that case, maybe set vp as centroid4f_base instead of -centroid4f_base might be sufficient?

Nevertheless, if we calculate d, the distance from origin to the plane correctly, and if the normal vector is pointing toward the plane, d will be negative. So maybe flipping the entire thing is enough?

[BillWSY]

Also, the original code makes sense because if a plane's boundary point is not on the plane -- it won't make sense -- and something terrible must have happened -- time to crash.

[ruffsl]

With https://github.com/CogRob/omnimapper/pull/29 , looks like this is working now.