Open Naoki-Hiraoka opened 6 years ago
YutoUchimi
commented
6 years ago 白や青の点はちらついたりするのでしょうか？
青色が混じるのとフィルター重ねがけで改善されるのはよくわからないけど、ちらつくとすれば白い点群が残るのはもしかするとパラメータに原因があるかもしれませんね。 HLS色空間(=HSI色空間)は円柱モデルや双円錐モデルで表現されますが、僕の理解ではHやSの値にかかわらずL(=I)は0が黒、127が純色、255が白を指すはずなので、hsi_filterノードのパラメータi_limit_maxを例えば200くらいに下げると白は消せるのではないかと思います。
Naoki-Hiraoka
commented
6 years ago ありがとうございます。
白や青の点はちらついています。 動作しているjsk_pcl/HSI_ColorFilterがひとつだけの時は一切ちらつかない(白や青の点は現れない)にも関わらず、複数になった途端ちらつき始めます。
Lの上限、下限を設定して実行しましたが、白や青や茶の点がちらつきました。
<launch>
<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="\
screen" />
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid\
pcl_manager" output="screen">
<remap from="~input" to="/camera/depth_registered/points" />
<rosparam>
filter_field_name: z
filter_limit_min: 0.5
filter_limit_max: 1.5
filter_limit_negative: False
leaf_size: 0.01
</rosparam>
</node>
<node pkg="nodelet" type="nodelet" name="hsi_filter_green" respawn="true"
args="load jsk_pcl/HSIColorFilter pcl_manager" output="screen">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
use_indices: false
</rosparam>
<param name="h_limit_max" value="120" />
<param name="h_limit_min" value="70" />
<param name="s_limit_max" value="255" />
<param name="s_limit_min" value="50" />
<param name="i_limit_max" value="200" />
<param name="i_limit_min" value="50" />
</node>
<node pkg="nodelet" type="nodelet" name="hsi_filter_red" respawn="true"
args="load jsk_pcl/HSIColorFilter pcl_manager" output="screen">
<remap from="~input" to="voxel_grid/output" />
<rosparam>
use_indices: false
</rosparam>
<param name="h_limit_max" value="10" />
<param name="h_limit_min" value="-10" />
<param name="s_limit_max" value="255" />
<param name="s_limit_min" value="150" />
<param name="i_limit_max" value="200" />
<param name="i_limit_min" value="50" />
</node>
<node pkg="nodelet" type="nodelet" name="hsi_filter_green2" respawn="true"
args="load jsk_pcl/HSIColorFilter pcl_manager" output="screen">
<remap from="~input" to="hsi_filter_green/output" />
<rosparam>
use_indices: false
</rosparam>
<param name="h_limit_max" value="120" />
<param name="h_limit_min" value="70" />
<param name="s_limit_max" value="255" />
<param name="s_limit_min" value="50" />
<param name="i_limit_max" value="200" />
<param name="i_limit_min" value="50" />
</node>
</launch>結果
voxel_grid/output
hsi_filter_green/output(his_filter_redはsubscribeされていないので動作していないと思われる)
hsi_filter_green/output(横でhis_filter_red/outputをrostopic echoしている)
hsi_filter_green2/output(横でhis_filter_red/outputをrostopic echoしている)
YoheiKakiuchi
commented
6 years ago なんだか根深そうな問題に思えてきました。
jsk_pcl_ros/src/color_filter_nodelet.cppにデバッグプリントを仕込む。
template <class PackedComparison, typename Config>
void ColorFilter<PackedComparison, Config>::filter(const sensor_msgs::PointCloud2ConstPtr &input,
const PCLIndicesMsg::ConstPtr& indices)
{
boost::mutex::scoped_lock lock (mutex_);
pcl::PointCloud<pcl::PointXYZRGB> tmp_in, tmp_out;
sensor_msgs::PointCloud2 out;
fromROSMsg(*input, tmp_in);
{
int redp = 0;
int grnp = 0;
for (int i = 0; i < tmp_in.points.size(); i++) {
pcl::PointXYZRGB &pt = tmp_in.points[i];
int rgb = *(int *)(&(pt.rgb));
int red = ((0x00FF0000 & rgb) >> 16);
int grn = ((0x0000FF00 & rgb) >> 8);
if (red > 128) redp++;
if (grn > 128) grnp++;
}
ROS_INFO("before input: %d, red = %d, grn = %d", tmp_in.points.size(), redp, grnp);
}
filter_instance_.setInputCloud (tmp_in.makeShared());
if (indices) {
pcl::IndicesPtr vindices;
vindices.reset(new std::vector<int> (indices->indices));
filter_instance_.setIndices(vindices);
}
filter_instance_.filter(tmp_out);
{
int redp = 0;
int grnp = 0;
for (int i = 0; i < tmp_in.points.size(); i++) {
pcl::PointXYZRGB &pt = tmp_in.points[i];
int rgb = *(int *)(&(pt.rgb));
int red = ((0x00FF0000 & rgb) >> 16);
int grn = ((0x0000FF00 & rgb) >> 8);
if (red > 128) redp++;
if (grn > 128) grnp++;
}
ROS_INFO("after input: %d, red = %d, grn = %d", tmp_in.points.size(), redp, grnp);
}
{
int redp = 0;
int grnp = 0;
for (int i = 0; i < tmp_out.points.size(); i++) {
pcl::PointXYZRGB &pt = tmp_out.points[i];
int rgb = *(int *)(&(pt.rgb));
int red = ((0x00FF0000 & rgb) >> 16);
int grn = ((0x0000FF00 & rgb) >> 8);
if (red > 128) redp++;
if (grn > 128) grnp++;
}
ROS_INFO("input: %d, output: %d, red = %d, grn = %d", tmp_in.points.size(), tmp_out.points.size(), redp, grnp);
}
if (tmp_out.points.size() > 0) {
toROSMsg(tmp_out, out);
pub_.publish(out);
}
}以下が、デバッグプリント。
[ INFO] [1514438470.379895668]: before input: 523, red = 265, grn = 258
[ INFO] [1514438470.379948575]: before input: 523, red = 265, grn = 258
[ INFO] [1514438470.380068116]: after input: 523, red = 265, grn = 258
[ INFO] [1514438470.380097870]: input: 523, output: 224, red = 223, grn = 1
[ INFO] [1514438470.380130055]: after input: 523, red = 265, grn = 258
[ INFO] [1514438470.380157763]: input: 523, output: 283, red = 25, grn = 258before input と after input は常に同じなので、inputは問題ない。 outputの点はredかgreenのどちらかにならなければいけないのに、 redの点は少ない。greenの点は数はあっている(258個)が、redが混じっている。 確率的に、数は正しい時もあるし、間違っている時もある。
傾向としては、正しいフィルタ結果の点が多く、少し間違いが混じる。 報告の通りで、ノードが同時に動いていなければ、間違いは生じない。
pclのレベルのinput/outputで間違っていて、jskの書いたコードの部分も少ないので、 pcl本体のconditionalremovableがスレッドセーフでない可能性が高い気がするが追いきれていない。
参考 2色の色付き点群を出すeusのコード
#!/usr/bin/env roseus
;;(setq ros::*compile-message* t)
(ros::roseus-add-msgs "sensor_msgs")
(setq *points*
(let ((redp (make-random-pointcloud :num 300 :with-color t))
(grnp (make-random-pointcloud :num 300 :with-color t)))
(let ((cls (send redp :colors)))
(dotimes (i (array-dimension cls 0))
(setf (matrix-row cls i) (float-vector 1 0 0))))
(let ((cls (send grnp :colors)))
(dotimes (i (array-dimension cls 0))
(setf (matrix-row cls i) (float-vector 0 1 0))))
(send redp :translate (float-vector 80 0 800))
(send grnp :translate (float-vector -80 0 800))
(send redp :convert-to-world :create nil)
(send grnp :convert-to-world :create nil)
(send redp :append (list grnp) :create nil)
redp
))
(setq *points-sub* nil)
(defun callback ( type msg )
;; ROSのメッセージをeuslispのpointcloudに変換
(let (points)
(setq points (make-eus-pointcloud-from-ros-msg
msg :remove-nan t)) ;; for replace -> :remove-nan :replace
(when (and points (> (send points :size) 0))
(setf (get points :header) (send msg :header))
(setf (get points :type) type)
(push points *points-sub*))
))
(ros::roseus "subscribe_pointcloud")
(ros::advertise "/camera/depth_registered/points" sensor_msgs::PointCloud2)
;;(ros::subscribe "/voxel_grid/output" sensor_msgs::PointCloud2 #'callback :voxel 100)
;;(ros::subscribe "/hsi_filter_green/output" sensor_msgs::PointCloud2 #'callback :green 100)
;;(ros::subscribe "/hsi_filter_red/output" sensor_msgs::PointCloud2 #'callback :red 100)
;;(ros::subscribe "/hsi_filter_green2/output" sensor_msgs::PointCloud2 #'callback :green2 100)
(unix::sleep 1)
(setq ret nil)
(defun start-subscribe ()
(ros::rate 10)
(do-until-key
(ros::spin-once)
(pprint (length *points-sub*))
(let ((msg (make-ros-msg-from-eus-pointcloud *points* :with-color :rgb
:with-normal nil :frame "world")))
(ros::publish "/camera/depth_registered/points" msg))
(ros::sleep)
))
(start-subscribe)
sensor_msgs/pointcloud2のtopicに対し複数のjsk_pcl/HSI_ColorFilterを動作させると、それぞれの出力に、指定した範囲外の色相の点がいくつか交じるようになりました。 これらの点はひとつだけのjsk_pcl/HSI_ColorFilterを動作させた時には出力されなかったものです。
launchファイル
結果 rvizに/camera/depth_registered/pointsを表示。(元画像)。
rvizに一つのfilter(緑色を取り出すhsi_filter)の結果を表示。(もうひとつのfilterは、subscribeされていないので動いていないと思われる)
別の端末で別のfilter(赤を取り出すhsi_filter_red)の結果をrostopic echoさせる。この時、rvizにfilter(緑色を取り出すhsi_filter)の結果を表示。(1つのtopicに対し2つのfilterが動いている状態)
↑白や青の点が混ざっている。
使用しているPCは演習用PCです。 一つのtopicに複数のjsk_pcl/HSI_ColorFilterが作用するのがいけないのかと考え、間にpassthroughを挟んでみましたが、改善しませんでした。 同じfilterを重ねがけすることで、指定した範囲外の色相の点は出力にほぼ入らなくなり、この出力を使ったjsk_pcl/RegionGrowingMultiplePlaneSegmentationが望んだ結果になるようになりましたが、完全とは言えませんでした。
原因は何でしょうか。