Force plane coefficient to direct origin in plane concatinator

[Tacha-S]

Fixed a problem in PlaneConcatinator that caused the coefficients to flip depending on the angle between the point cloud frame and the plane.

• fix Plane coefficients direction is unstable in PlaneConcatenator. #2655

Before the improvement, the Z axis is inverted depending on the angle between the point cloud frame and the plane, as shown in the following pictures.
Since the normal vector of the plane in the pictures is calculated from the polygon without using coefficients, the orientation of the coefficients is determined by the centroid_pose_array of the ClusterPointIndicesDecomposer.

The improved version will look like the following

The nodelet pipeline used to check this behavior is shown below.

<launch>
  <arg name="color_input" default="/hand_camera/color/image_rect_color" />
  <arg name="cloud_input" default="/hand_camera/depth_registered/points" />
  <arg name="manager" default="primitive_shape_classifier_nodelet_manager" />
  <arg name="vital_rate" default="0.1" />
  <arg name="sensor_frame_id" default="/hand_camera_rgb_optical_frame" />
  <arg name="debug" default="false"/>

  <node name="$(arg manager)"
        pkg="nodelet" type="nodelet" args="manager" output="screen" />

  <group ns="primitive_shape_classifier">
    <node name="input_relay" pkg="nodelet" type="nodelet"
          args="load jsk_topic_tools/Relay /$(arg manager)">
      <remap from="~input" to="$(arg cloud_input)" />
    </node>
    <node name="bilateral_filter" pkg="nodelet" type="nodelet"
          args="load jsk_pcl/BilateralFilter /$(arg manager)">
      <remap from="~input" to="input_relay/output" />
      <rosparam subst_value="true">
        sigma_s: 1.0
        sigma_r: 0.1
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
    <node name="organized_multi_plane_segmentation" pkg="nodelet" type="nodelet"
        args="load jsk_pcl/OrganizedMultiPlaneSegmentation /$(arg manager)">
      <remap from="~input" to="bilateral_filter/output" />
      <rosparam subst_value="true">
        max_curvature: 0.01
        estimate_normal: true
        publish_normal: true
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
    <node name="plane_concatenator" pkg="nodelet" type="nodelet"
          args="load jsk_pcl/PlaneConcatenator /$(arg manager)">
      <remap from="~input" to="bilateral_filter/output" />
      <remap from="~input/indices" to="organized_multi_plane_segmentation/output_refined" />
      <remap from="~input/polygons" to="organized_multi_plane_segmentation/output_refined_polygon" />
      <remap from="~input/coefficients" to="organized_multi_plane_segmentation/output_refined_coefficients" />
      <rosparam subst_value="true">
        connect_distance_threshold: 0.2
        min_area: 0.3
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
    <node name="multi_plane_extraction" pkg="nodelet" type="nodelet"
          args="load jsk_pcl/MultiPlaneExtraction /$(arg manager)">
      <remap from="~input" to="bilateral_filter/output" />
      <remap from="~indices" to="plane_concatenator/output/indices" />
      <remap from="~input_polygons" to="plane_concatenator/output/polygons" />
      <remap from="~input_coefficients" to="plane_concatenator/output/coefficients" />
      <rosparam subst_value="true">
        use_sensor_frame: true
        sensor_frame: $(arg sensor_frame_id)
        min_height: 0.01
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
    <node name="euclidean_clustering" pkg="nodelet" type="nodelet"
          args="load jsk_pcl/EuclideanClustering /$(arg manager)">
      <remap from="~input" to="multi_plane_extraction/output" />
      <rosparam subst_value="true">
        tolerance: 0.02
        min_size: 100
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
    <node name="euclidean_segmentation_decomposer" pkg="nodelet" type="nodelet"
          args="load jsk_pcl/ClusterPointIndicesDecomposer /$(arg manager)">
      <remap from="~input" to="multi_plane_extraction/output" />
      <remap from="~target" to="euclidean_clustering/output" />
      <remap from="~align_planes" to="plane_concatenator/output/polygons" />
      <remap from="~align_planes_coefficients" to="plane_concatenator/output/coefficients" />
      <rosparam subst_value="true">
        align_boxes: true
        align_boxes_with_plane: true
        fill_boxes_label_with_nearest_plane_index: true
        use_pca: true
        publish_clouds: false
        publish_tf: false
        vital_rate: $(arg vital_rate)
      </rosparam>
    </node>
  </group>
</launch>

[iory]

Is it enough to change the orientation of fixed_refined_coefficients without changing the orientation of the original_coefficients?

[Tacha-S]

I think it depends on the nodelet design.
One thing I noticed is that the RANSAC estimation has a different direction of the coefficients depending on the angle of the plane, so I only do it if the plane is refined by RANSAC.

If the output should be aligned to point close to the origin for any input, then the original_coefficients should also change direction, but in that case, I think we need to make sure that the order of the points in the polygon is also reversed.
On the other hand, I don't think there is any need to change the original_coefficients if the input is assumed to have the same orientation of the coefficients as the orientation of the vertical vector derived from the order of the polygon points.

[iory]

Thank you for your explanation.
Could you add the explanation for the direction of coefficients in the following document?
https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/doc/jsk_pcl_ros_utils/nodes/plane_concatenator.md

[iory]

Thank you!