[jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink #2053

furushchev · 2017-04-17T01:01:55Z

allow negative value for magnify_distance param to shrink polygons.
updated docs with an image
fixed a bug to drop labels and likelihood.

wkentaro · 2017-04-19T23:55:43Z

jsk_pcl_ros_utils/src/polygon_magnifier_nodelet.cpp

+
+    ret_polygon_array.likelihood.reserve(msg->likelihood.size());
+    std::copy(msg->likelihood.begin(), msg->likelihood.end(),
+              std::back_inserter(ret_polygon_array.likelihood));


@furushchev Could you please explain what these lines do?

@wkentaro Thanks for reviewing! What is done in this part is just to copy all elements of vector. Original code drops labels and likelihood from original message even if they have some values.

ref:

fixed a bug to drop labels and likelihood.

Why you can't firstly copy all elements from input msg?

@wkentaro Because I didn't know the way. Updated.

wkentaro · 2017-04-20T01:23:00Z

Please add sample and test, refering to previous works:

furushchev · 2017-04-21T10:00:38Z

@wkentaro Thanks for very kind tutorials! Updated.

wkentaro · 2017-04-21T13:00:09Z

jsk_recognition_utils/src/geo/convex_polygon.cpp

-      new_vertices[i] = (vertices_[i] - c).normalized() * distance + vertices_[i];
+      Eigen::Vector3f diff_vec;
+      if (distance > 0.0) diff_vec = vertices_[i] - c;
+      else                diff_vec = c - vertices_[i];


distance is not used in this if block.
could u explain why?

@wkentaro instead abs(distance) is multiplied. Original code may make polygons which has opposite directed normals. (I could not know why though)

abs(distance) is out of if block, right?

I mean why the sign of distance decides the sign of diff_vec.

@wkentaro Please read the code, if distance is negative, we need to compute a vector of opposite direction.

I asked @mmurooka for review.

I double checked with @wkentaro ,
and this change is not necessary (i.e. this does not change behavior at all).
Because if distance is negative
diff_vec is c - vertices_[i], and diff_vec * abs(distance) becames diff_vec * (- distance) = (vertices_[i] - c) * distance.
This is same result with original source code.

abs returns integer. please use fabs.

this part was reverted and no abs is used any more. Thanks anyway.

wkentaro · 2017-04-21T13:00:59Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Copyright: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>


Please add license. BSD, MIT, or something.

But I could not find other nodes that have license header.
Even if this is preferred, I think we should create another PR to update it for all these nodes.

In other nodes, the license is estimated by package.xml (probably BSD), but you specify copyright, and I thought it was under another license.

If you write Author: ..., I can ignore it.

Sorry, this line is automatically added by emacs. Removed.

furushchev · 2017-04-21T15:25:57Z

Yes, but result is different.(in original code, normal direction of plane is sometimes flipped) I don't know why. Do you know.?? I'll give you rosbag if you want to see the behavior.

wkentaro · 2017-04-21T15:20:15Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

+    def publish(self, event):
+        # update timestamp
+        self.msg.header.stamp = event.current_real
+        for p in self.msg.polygons: p.header.stamp = event.current_real


Please add newline.

wkentaro · 2017-04-21T15:21:36Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

+        # validate params
+        assert isinstance(param, list), "polygons must be list"
+
+        has_label = any(map(lambda p: "label" in p, param))


has_label = any('label' in p for p in param)

wkentaro · 2017-04-21T15:22:01Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

+        assert isinstance(param, list), "polygons must be list"
+
+        has_label = any(map(lambda p: "label" in p, param))
+        has_likelihood = any(map(lambda p: "likelihood" in p, param))


has_likelihood = any('likelihood' in p for p in param)

wkentaro · 2017-04-21T15:22:26Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

+        for polygon in param:
+            ps = PolygonStamped()
+            ps.header.frame_id = frame_id
+            ps.polygon.points = map(lambda v: Point32(v[0], v[1], v[2]), polygon["points"])


[Point32(*v) for v in polygon['points']]

mmurooka · 2017-04-21T16:03:51Z

Yes, but result is different.(in original code, normal direction of plane is sometimes flipped) I don't know why. Do you know.??

I don't know. Please describe normal direction of plane is sometimes flipped in more detail.
Do you check that in Rviz or polygon instance of jsk_recognition_utils/geo in C++ code?
I think that distance < 0 and |distance| is larger than input polygon size, the clockwise/counterclockwise of magnified polygon vertices might be flipped and then normal flipped.

mmurooka · 2017-04-21T16:25:07Z

I executed sample_polygon_magnifier.launch with this PR update, and it worked (normal was not flipped).
Then, I reverted

jsk_pcl_ros_utils/src/polygon_magnifier_nodelet.cpp
jsk_recognition_utils/src/geo/convex_polygon.cpp

to origin/master, then build and executed same sample launch, and it also worked (normal was not flipped).
I need rosbag to check more. Could you give rosbag (or could you check once again) please?

mmurooka · 2017-04-21T16:36:10Z

Sorry for many reply comments.
I found one possible reason that normal is flipped.
The shrunk polygon might become non-convex even if the input polygon is convex because the polygon is shrunk with constant distance for all verticies (not ratio).
In the readme figure you add is good example. Green shrunk polygon is not convex if you see carefully.
If two edges of non-convex parts are used for calculating normal, normal is flipped from original. Is that make sense?

This can happen regardless of your change in magnifyByDistance. My guess is that your experimental environment was changed slightly at the same timing that you updated the code.

mmurooka · 2017-04-21T17:25:46Z

master...mmurooka:polygon-magnifier-normal-flip reproduce normal flipping.

furushchev · 2017-04-22T00:37:06Z

@mmurooka You are totally right!! Thanks! I'll write a code to warn if breaks original convex hull.

Green shrunk polygon is not convex if you see carefully.
If two edges of non-convex parts are used for calculating normal, normal is flipped from original. Is that make sense?

Sorry, if I understand correctly, you mean Red? (not Green).

furushchev · 2017-04-22T01:06:43Z

@wkentaro @mmurooka Updated thanks to your many advices. If you approve, I rebase all commits into one.

mmurooka · 2017-04-22T03:08:02Z

jsk_pcl_ros_utils/src/polygon_magnifier_nodelet.cpp

+        = poly.magnifyByDistance(magnify_distance_);
+
+      if (!magnified_poly->isConvex()) {
+        ROS_ERROR("Magnified polygon %ld is not convex.", i);


I think it is OK to push magnified polygon even if it is non-convex. In some cases, we want the result even if it is non-convex. Keeping length of polygon array is easy to manage.
How about following (just add warn message if non-convex)?

jsk_recognition_msgs::PolygonArray ret_polygon_array = *msg; for (size_t i = 0; i < msg->polygons.size(); i++) { jsk_recognition_utils::ConvexPolygon poly = jsk_recognition_utils::ConvexPolygon::fromROSMsg(msg->polygons[i].polygon); jsk_recognition_utils::ConvexPolygon::Ptr magnified_poly = poly.magnifyByDistance(magnify_distance_); if (!magnified_poly->isConvex()) { ROS_WARN("Magnified polygon %ld is not convex.", i); } ret_polygon_array.polygons[i].polygon = magnified_poly->toROSMsg(); } pub_.publish(ret_polygon_array);

mmurooka · 2017-04-22T03:10:50Z

Sorry, if I understand correctly, you mean Red? (not Green).

I'm saying about this figure in this comment. https://github.com/furushchev/jsk_recognition/blob/69b0e4c079bc35200d0862cd8be400252899c15a/doc/jsk_pcl_ros_utils/nodes/images/polygon_magnifier.png
green polygon is a little bit non-convex.

mmurooka · 2017-04-22T03:17:18Z

If keeping convex (and not flipping normal) is important for your use case, I recommend you to use magnify by scale in nodelet, and add rosparam to select magnifying by distance or scale.
https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/jsk_recognition_utils/src/geo/convex_polygon.cpp#L261
This is just a comment, and you don't need to do this in this PR.

furushchev · 2017-04-22T03:31:35Z

@mmurooka Thanks for advice, updated / rebased and yes, I'll send another pull request to support magnify by scale factor when we want.

mmurooka · 2017-04-22T04:19:43Z

LGTM!

wkentaro · 2017-04-22T04:37:21Z

jsk_recognition_utils/node_scripts/polygon_array_publisher.py

+    def publish(self, event):
+        # update timestamp
+        self.msg.header.stamp = event.current_real
+        for p in self.msg.polygons: p.header.stamp = event.current_real


wkentaro · 2017-04-22T04:38:46Z

doc/jsk_pcl_ros_utils/nodes/polygon_magnifier.md

@@ -14,4 +17,10 @@ Magnify polygons by specified length.
 ## Parameters
 * `~magnify_distance` (Double, default: `0.2`)

-  Length to scale polygon
+  Length to scale polygons. Default value `0.2` means the distance of each corresponding edges will be `0.2` m.


Please describe that this nodelet supports negative value.

Updated.
Sorry I added newline in different line.

…nify [jsk_pcl_ros_utils][polygon_magnifier] update docs [jsk_recognition_utils] add polygon_array_publisher.py / sample_polygon_array_publisher.launch [jsk_pcl_ros_utils] add sample / test for polygon_magnifier

furushchev · 2017-04-22T13:13:04Z

Hmm, failed on indigo...

20:29:28   Could NOT find Cython (missing: CYTHON_EXECUTABLE)

20:29:28 Call Stack (most recent call first):

20:29:28   /usr/share/cmake-2.8/Modules/FindPackageHandleStandardArgs.cmake:315 (_FPHSA_FAILURE_MESSAGE)

20:29:28   cmake/FindCython.cmake:42 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)

20:29:28   cmake/UseCython.cmake:74 (find_package)

20:29:28   CMakeLists.txt:47 (include)

furushchev · 2017-04-22T13:57:39Z

OK. Test passed!

furushchev changed the title ~~[jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink~~ [WIP][jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink Apr 17, 2017

furushchev force-pushed the polygon-magnifier branch from 330c27b to 44a78a8 Compare April 17, 2017 01:25

furushchev changed the title ~~[WIP][jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink~~ [jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink Apr 17, 2017

wkentaro added feature pkg/jsk_pcl_ros labels Apr 19, 2017

wkentaro reviewed Apr 19, 2017

View reviewed changes

wkentaro self-assigned this Apr 19, 2017

furushchev force-pushed the polygon-magnifier branch from 44a78a8 to 3428d49 Compare April 20, 2017 01:01

wkentaro reviewed Apr 21, 2017

View reviewed changes

furushchev force-pushed the polygon-magnifier branch from 0402cbf to 0c7c2bd Compare April 21, 2017 13:36

wkentaro approved these changes Apr 21, 2017

View reviewed changes

furushchev mentioned this pull request Apr 22, 2017

[jsk_pcl_ros_utils] add PolygonArrayLikelihoodFilter #2054

Merged

mmurooka approved these changes Apr 22, 2017

View reviewed changes

furushchev force-pushed the polygon-magnifier branch from 69b0e4c to 05920be Compare April 22, 2017 03:30

wkentaro requested changes Apr 22, 2017

View reviewed changes

[jsk_pcl_ros_utils][polygon_magnifier] allow negative distance to mag…

2a7b621

…nify [jsk_pcl_ros_utils][polygon_magnifier] update docs [jsk_recognition_utils] add polygon_array_publisher.py / sample_polygon_array_publisher.launch [jsk_pcl_ros_utils] add sample / test for polygon_magnifier

furushchev force-pushed the polygon-magnifier branch from 05920be to 2a7b621 Compare April 22, 2017 05:12

wkentaro approved these changes Apr 22, 2017

View reviewed changes

furushchev mentioned this pull request Apr 28, 2017

[jsk_pcl_ros_utils][polygon_magnifier] Support scale factor to magnify polygon #2072

Merged

k-okada merged commit 508c672 into jsk-ros-pkg:master May 2, 2017

furushchev deleted the polygon-magnifier branch May 21, 2017 02:06

[jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink #2053

[jsk_pcl_ros_utils][polygon_magnifier] Allow negative param value to shrink #2053

Conversation

furushchev commented Apr 17, 2017 • edited Loading

Choose a reason for hiding this comment

furushchev Apr 20, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

wkentaro commented Apr 20, 2017 • edited Loading

furushchev commented Apr 21, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

wkentaro Apr 21, 2017 • edited Loading

Choose a reason for hiding this comment

mmurooka Apr 21, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

furushchev commented Apr 21, 2017 via email • edited by wkentaro Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mmurooka commented Apr 21, 2017

mmurooka commented Apr 21, 2017

mmurooka commented Apr 21, 2017

mmurooka commented Apr 21, 2017

furushchev commented Apr 22, 2017 • edited Loading

furushchev commented Apr 22, 2017

Choose a reason for hiding this comment

mmurooka commented Apr 22, 2017

mmurooka commented Apr 22, 2017

furushchev commented Apr 22, 2017

mmurooka commented Apr 22, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

furushchev commented Apr 22, 2017

furushchev commented Apr 22, 2017

furushchev commented Apr 17, 2017 •

edited

Loading

furushchev Apr 20, 2017 •

edited

Loading

wkentaro commented Apr 20, 2017 •

edited

Loading

wkentaro Apr 21, 2017 •

edited

Loading

mmurooka Apr 21, 2017 •

edited

Loading

furushchev commented Apr 21, 2017 via email •

edited by wkentaro

Loading

furushchev commented Apr 22, 2017 •

edited

Loading