OpenCV기반 객체 인식

OpenCV를 이용한 객체 인식은 고전적인 주제이지만, 아직 완벽히 해결되지는 않은 영역이기도 하다. 하긴, 모든 사물을 인식한다면, 터미네이터 개발이 어렵지 않겠지.

1. 개요
OpenCV는 비전 분야에서는 모르면, 간첩인 비전에 필요한 수많은 기능을 모아둔 오픈소스 라이브러리이다. 이 글에서는 다음 글을 참고한다.

• ROS wiki - vision_opencv
• ROS by Example: Head Tracking using OpenCV

XBee기반 홈 오토메이션 웹서버 개발

한동안 하지 못했던 스마트 홈 오토메이션 웹서버를 만들어 본다. 웹서버라고 해도 그리 대단한 건 아니다. node.js를 사용해 서버를 구현할 계획이다. 이 내용은 다음 레퍼런스를 참고한다.

• 아두이노 기반 스마트 홈 오토메이션 
• 홈 오토메이션 GitHub 소스
• Portable Wind Turbine

참고로, 특정 센서 값을 취득하여, 서버에 저장하고, 계산하거나, 웹을 통해, 릴레이, 액추에이터 등을 제어하는 것은 모두 이 글에서 기술하는 것과 유사한 방식으로 처리할 수 있다.

1. 개요

node.js는 JavaScript 런타임 상에 개발된 플래폼으로, 쉽고 빠르게, 확장성있는 네트워크 어플리케이션을 개발할 수 있도록 지원한다. node.js는 이벤트 드리븐 구조이며, non-blocking 입출력 모델을 지원하여, 가볍고 효율적인 데이터 중싱 실시간 어플리케이션을 분산된 장치 기반에서 실행할 수 있도록 개발을 지원한다. 

node.js로 얼마나 쉽게 네트워크 어플리케이션을 개발할 수 있는 지 보자.

보통 웹서버를 개발한다면, 예전 방식은, 아파치 혹은 IIS와 같은 웹서버와 각종 웹 어플리케이션 컨테이너 프레임웍을 설치한 후, 복잡한 설정값을 맞춰줘야 한다. 그리고, 자바, C#과 같은 언어를 컴파일하고, 각종 스크립트 및 마크업언어와 메쉬업하는 작업을 해야 하는 등, 여러가지 복잡하고 짜증나는 일들이 많았다.

이제 node.js로 웹서버 어플리케이션을 개발해보자.

우선 node.js를 본인 운영체제에 맞게 설치한다.

• node.js 다운로드

그리고, 다음 코드를 example.js 파일로 저장한다.

var http = require('http');
http.createServer(function (req, res) {
  res.writeHead(200, {'Content-Type': 'text/plain'});
  res.end('Hello World\n');
}).listen(1337, '127.0.0.1');
console.log('Server running at http://127.0.0.1:1337/');

그리고, 다음과 같이 터미널에서 실행한다.
node example.js

그리고, 클롬에서 http://localhost:1337/ 주소로 접근해 본다.

잘 동작하는 것을 확인할 수 있다.

매우 간단한 웹서버 어플리케이션을 개발해 보았다. 이런 방식으로 복잡한 기능을 지원하는 서버 프로그램도 가능하다.

2. 스마트 홈 웹서버 개발

1. XBee 개발
다음 GitHub에서 소스를 다운받아, 작업폴더 내에 압축을 풀어 놓는다.

• 홈 오토메이션 GitHub 소스

npm (node package manager) 를 이용해, 스마트 홈 서버 개발에 유용한 몇가지 패키지를 설치해 보겠다. 다음과 같이 터미널에서 명령을 입력해, 패키지를 설치한다.

PIR 모션 센서와 XBee 쉴드를 다음과 같이 연결한다.

1. XBee 쉴드와 아두이노를 연결한다.
2. PIR 모션센서 GND / VCC와 아두이노 GND / VCC(5V)연결.
3. PIR 모션센서 OUT은 아두이노 8번 핀에 연결
4. XBee 쉴드 스위치 on / Analog In 스위치를 DLINE 으로 설정

모션센서가 제대로 동작하는 지 확인하기 위해, 다음 아두이노 코드를 실행해 본다.

// Simple motion sensor
int sensor_pin = 8;
void setup() {
  Serial.begin(9600);
}
void loop() {
  // Read sensor data
  int sensor_state = digitalRead(sensor_pin);
  // Print data
  Serial.print("Motion sensor state: ");
  Serial.println(sensor_state);
  delay(100);
}

시리얼 모니터를 실행해 보면, 다음 결과값을 확인할 수 있다. 인체가 가까이 있을 경우, 센서 상태가 1로 켜지는 것을 확인할 수 있다.

2. 아두이노 코딩
웹서버와 JSON방식으로 센서 데이터 통신을 위해 다음의 아두이노 라이브러리를 설치한다.

https://github.com/marcoschwartz/aREST

아두이노 IDE에서 다음과 같이 코딩한다.

// Libraries
#include <SPI.h>
#include <aREST.h>

// Motion sensor ID
String xbee_id = "1";
// Create ArduREST instance
aREST rest = aREST();

void setup() {
  // Start Serial
  Serial.begin(9600);
  // Give name and ID to device
  rest.set_id(xbee_id);
}

void loop() {
  // Handle REST calls
  rest.handle(Serial);
}

이후, 시리얼 모니터를 열고, 다음 명령을 입력해, XBee볻에 현재 집안에 있는 모든 XBee를 리스트하도록 한다. 

/id

현재 XBee가 하나라면, 다음과 같이 XBee가 응답한다. 

{"id": "1", "name": "", "connected": true}

센서 8번핀의 값을 획득해 본다. 

/digital/8

그럼 다음과 같이 응답할 것이다.

{"return_value": 1, "id": "1", "name": "", "connected": true}

만약 동작하지 않은 경우, PAN(Personal Area Network) 동일 ID가 아닐 경우가 있으므로, 다음 링크에서 다운받아, 동일 ID로 설정한다. 

http://www.digi.com/products/wireless-wired-embedded-solutions/zigbee-rf-modules/xctu

3. 웹서버 개발

터미널에서 xbee_motion_sensors의 interface 폴더로 이동한 후 다음과 같이 aREST 모듈을 설치할 수 있도록 명령을 입력한다.

sudo npm install arest

윈도우 운영체계라면, 명령 앞의 sudo를 제외하고 입력한다.

sudo npm install serialport

express 모듈 설치를 위해 다음 명령을 입력한다.

sudo npm install express

마지막으로, Node.js 서버를 다음 명령으로 시작한다.

sudo node app.js

app.js의 내부 소스는 다음과 같다.

// Module

var express = require('express');

var path = require('path');

var arest = require('arest');

// Create app

var app = express();

var port = 3700;

// Set views

app.use(express.static(path.join(__dirname, 'public')));

app.use(express.static(path.join(__dirname, 'views')));

// Serve files

app.get('/interface', function(req, res){

res.sendfile('views/interface.html')

});

// API access

app.get("/send", function(req, res){

arest.send(req,res);

});

// Start server

app.listen(port);

           console.log("Listening on port " + port);

app.js는 다음과 같이, 클라이언트 접속요청이 들어오면, express객체인 app의 listen함수를 호출해, 3700포트로 서버 대기 상태가 된다.

만약, url 에 /interface를 입력하면, 파일안의 html을 send해 렌더링한다.
/send url로 접속했을 때는 arest로 해당 메시지를 전달한다. 이는 아두이노에 연결된 XBee보드에 시리얼로 해당 메시지가 전달되도록 한다.

app.js 실행 후에는 터미널에서 다음 메시지를 확인할 수 있다.

Listening on port 3700

3. 테스트

이제, 웹 브라우저를 실행해, 주소창에 다음을 입력한다.

localhost:3700/interface

그럼 다음과 같은 인터페이스를 확인할 수 있다.

4. 결론

node.js 로 네트워크 어플리케이션을 개발하는 것은 매우 쉽다. 아울러, 다양한 javascript 를 활용하여, 다양한 유저인터페이스를 구현할 수 있으며, 렌더링된 HTML은 스마트폰과 같은 다양한 장치에서 동일하게 보여지므로, N-Screen을 쉽게 구현할 수 있다.

참고로 Bluetooth 기반으로 개발하고자 한다면, 다음 링크를 참고한다.

• 스마트 홈 만들기 - 통신

ROS기반 액추에이터 제어

ROS기반으로 객체를 추적하기 위해서는 액추에이터에 적절한 속도를 전달해 줄 수 있어야 한다. 이와 관련된 내용을 정리해 본다. 본 글에서는 많이 활용되는 다이나믹셀을 활용한다.

1. 개요
이 글은 다음 글을 참고하였다.

• ROS dynamixel overview
• ROS dynamixel tutorial overview
• ROS dynamixel tutorial
• Head tracking
• Creating join position
• Dynamixel connection 설명

1. 액추에이터
본 글에서 사용하는 액추에이터는 다이나믹셀(dynamixel) AX-12W이다. 이에 대한 상세한 설명은 아래 링크를 참고 하길 바란다.

• 감속기란
• Dynamixel Servo Manual
• Dynamixel Servo

간단히 말해, AX-12W는 저렴한 감속기, 제어기 및 네트워크 기능을 하나로 패키징한 액추에이터이다. 보통, 서보 모터로 감속기, 제어기 및 네트워크 기능을 포함하려면, 아두이노와 같은 별도의 보드를 붙여, 이런 것들을 구현해야 한다. 이를 패키징했다고 생각하면 된다. 

무게는 52.9g, 사용 전압은 9~12V (11.1V 권장), 프로토콜은 시리얼 통신을 사용하며, 위치센서로 Potentiometer가 사용된다. 

2. OpenCM

AX-12W를 구동하기 위해서, OpenCM 보드를 사용한다. 이 보드는 로보티즈에서 개발된 아두이노와 유사한 보드이며, 개발 IDE역시, 아두이노 IDE와 매우 유사하다.

2. ROS 패키지 설명
ROS Robotis Dynamixel 패키지는 AX-12, AX-18, RS-24, RX-28, MX-28, RX-64, MX-64, EX-106, MX-106 모델을 지원한다.

소스는 github에서 다운로드 가능하다.
https://github.com/arebgun/dynamixel_motor.git

설치는 다음과 같다.
sudo apt-get install ros-indigo-dynamixel-motor

최신버전 체크는 다음과 같다.
git clone https://github.com/arebgun/dynamixel_motor.git

이 패키지의 노드에서 지원하는 서비스는 다음과 같다. 상세 내용은 wiki와 API 를 참고한다.

• RestartController
• SetComplianceMargin
• SetCompliancePunch
• SetComplianceSlope
• SetSpeed
• SetTorqueLimit
• StartController
• StopController
• TorqueEnable

2. 튜토리얼
1. 패키지를 생성한다.

cd ~/catkin_ws/src
catkin_create_pkg my_dynamixel_tutorial dynamixel_controllers std_msgs rospy roscpp

다이나믹셀은 /dev/ttyUSB0 시리얼포트에 연결되었다고 가정한다.

2. 먼저, 모터 연결 및 피드백 데이터(예 - 현재 위치, 목표 위치, 에러 등등)를 publish하기 위해, 제어기 관리자(controller manager)를 시작할 필요가 있다. 간단한 방법은 launch 파일에 관련 파라메터를 설정하는 것이다. controller_manager.launch 파일에 다음 텍스트를 붙여 넣는다.

https://ua-ros-pkg.googlecode.com/svn/stacks/dynamixel_motor/tags/fuerte/dynamixel_tutorials/launch/controller_manager.launch

<!-- -*- mode: XML -*- -->

<launch>
    <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
        <rosparam>
            namespace: dxl_manager
            serial_ports:
                pan_tilt_port:
                    port_name: "/dev/ttyUSB0"
                    baud_rate: 1000000
                    min_motor_id: 1
                    max_motor_id: 25
                    update_rate: 20
        </rosparam>
    </node>
</launch>

baud rate 등을 정확히 설정한다. 만약 RX-28 모터라면, 57142로 설정해야 한다.

이제 다음과 같은 출력을 볼 수 있다.
[INFO] [WallTime: 1295282870.051953] pan_tilt_port: Pinging motor IDs 1 through 25...
[INFO] [WallTime: 1295282870.103676] pan_tilt_port: Found 4 motors - 4 AX-12 [4, 5, 6, 13], initialization complete. 

3. 모터 피드백 확인
토픽을 확인한다.
rostopic list

그럼 다음과 같이 출력될 것이다.
/motor_states/pan_tilt_port
/rosout
/rosout_agg

다음과 같이 입력한다.
rostopic echo /motor_states/pan_tilt_port

AX-12라면, 다음과 같이 출력된다.
motor_states:
motor_states:
  -
    timestamp: 1351555461.28
    id: 4
    goal: 517
    position: 527
    error: 10
    speed: 0
    load: 0.3125
    voltage: 12.4
    temperature: 39
    moving: False
  -
    timestamp: 1351555461.28
    id: 5
    goal: 512
    position: 483
    error: -29
    speed: 0
    load: 0.0
    voltage: 12.6
    temperature: 40
    moving: False

다음 튜토리얼도 시도해 본다.

1. Creating a joint controller
   This tutorial describes how to create a joint controller with one or more Robotis Dynamixel motors.
2. Creating a dual joint position controller
   This tutorial describes how to create a dual joint position controller with a Robotis Dynamixel motors.
3. Creating a joint torque controller
   This tutorial describes how to create a joint torque controller with a Robotis Dynamixel motor.
4. Creating a dual joint torque controller
   This tutorial describes how to create a dual joint torque controller with a Robotis Dynamixel motors.
5. Creating a dynamixel action client controller
   This tutorial describes how to create a joint controller with one or more Robotis Dynamixel motors.

2. Dynamixel for Arduino기반 다이나믹셀 제어

다음 링크에서 라이브러리를 다운로드 받고, 아두이노 라이브러리에 설치한다.

• Dynamixel for Arduino 라이브러리

상세 내용은 여기를 참고한다.
라이브러리 함수 설명은 여기를 참고한다.
아두이노와 다이나믹셀 연결 방법은 여기를 참고한다.
74LS241칩을 이용한 RX, TX연결 방법은 여기를 참고한다.
다이나믹셀 구조는 여기를 참고한다.

예제를 실행해 본다.

기타 예제는 다음을 참고한다.

• AX-12 and Flex force sensor & library download
• Dynamixel serial library download#1 or #2
• Dynamixel AX-12A and Arduino: how to use the Serial Port

3. 결론

OpenCM은 가끔 드라이버가 제대로 설치안된다던지 하는 문제가 있을 수 있다. 이 경우, 가지고 있는 아두이노 보드를 이용해, 다이나믹셀을 제어해 볼 수 있다.

3차원 포인트 클라우드 스캔 기반 객체 추적

이 글은 앞의 내용을 종합해, 3차원 포인트 클라우드를 입력받아, 스캔 기반으로 객체를 추적하는 방법을 개발해 본다.

1. 개요
객체를 추적하기 위해서는 포인트 클라우드를 적절히 세그먼테이션해야 하며, 각 세그먼트의 물성(중심점, AABB 정보 등)을 추출해야 한다. 각 세그먼트는 RANSAC등의 방법을 통해, 평면, 실린더 등 객체로 구분할 수 있다.

여기서는 단순히, 유클리디언 거리로 포인트 클라우드를 세그먼테이션하고, 세그먼트의 중심점 및 최대 영역을 획득해 본다.

2. 코딩
앞의 글과 같이 PCL 패키지를 생성해 보고, node의 소스를 다음과 같이 코딩한다.

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
// #include <pcl/features/moment_of_inertia_estimation.h>

ros::Publisher pub, pub_obj;

typedef pcl::PointXYZ PointT;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);
 
  // Data containers used
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  vg.setInputCloud (cloud.makeShared());
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl;

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);
 

  std::cout << "Number of clusters is equal to " << cluster_indices.size () << std::endl;
 
  // pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
 
  pcl::PointCloud<pcl::PointXYZI> TotalCloud, ObjectCloud; 
  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
      pcl::PointCloud<pcl::PointXYZI>::Ptr segment(new pcl::PointCloud<pcl::PointXYZI> ());

        pcl::PointXYZI min_point_AABB, max_point_AABB, mass_center;
        // Eigen::Vector3f mass_center;
        min_point_AABB.x = min_point_AABB.y = min_point_AABB.z = 10000000000.0;
      max_point_AABB.x = max_point_AABB.y = max_point_AABB.z = -10000000000.0;

    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
        pcl::PointXYZ pt = cloud_filtered->points[*pit];
            pcl::PointXYZI pt2;
            pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
            pt2.intensity = (float)(j + 1);

            TotalCloud.push_back(pt2);
            segment->push_back(pt2);
           
            min_point_AABB.x = std::min(min_point_AABB.x, pt.x);
            min_point_AABB.y = std::min(min_point_AABB.y, pt.y);
            min_point_AABB.z = std::min(min_point_AABB.z, pt.z);
            min_point_AABB.intensity = (float)(j + 1);

            max_point_AABB.x = std::max(max_point_AABB.x, pt.x);
            max_point_AABB.y = std::max(max_point_AABB.y, pt.y);
            max_point_AABB.z = std::max(max_point_AABB.z, pt.z);
            max_point_AABB.intensity = (float)(j + 1);           
    }
         
        /* feature_extractor.setInputCloud (segment);
        feature_extractor.compute ();


        feature_extractor.getAABB (min_point_AABB, max_point_AABB); 
        feature_extractor.getMassCenter (mass_center);   */
       
        mass_center.x = (max_point_AABB.x - min_point_AABB.x) / 2.0 + min_point_AABB.x;
        mass_center.y = (max_point_AABB.y - min_point_AABB.y) / 2.0 + min_point_AABB.y;
        mass_center.z = (max_point_AABB.z - min_point_AABB.z) / 2.0 + min_point_AABB.z;
        mass_center.intensity = (float)(j + 1);               
                       
        ObjectCloud.push_back(min_point_AABB);
        ObjectCloud.push_back(max_point_AABB);
        ObjectCloud.push_back(mass_center);
   
    j++;
  }

    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(TotalCloud, cloud_p);
  
  sensor_msgs::PointCloud2 output; 
  pcl_conversions::fromPCL(cloud_p, output);
  output.header.frame_id = "camera_depth_optical_frame";   
  pub.publish(output);

    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p2;
  pcl::toPCLPointCloud2(ObjectCloud, cloud_p2);
  
  sensor_msgs::PointCloud2 output2; 
  pcl_conversions::fromPCL(cloud_p2, output2);
  output2.header.frame_id = "camera_depth_optical_frame";   
  pub_obj.publish(output2);   

 
  ROS_INFO("published it."); 
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);

  // Create a ROS publisher for the segments
  pub = nh.advertise<sensor_msgs::PointCloud2> ("pclsegments", 1);

  // Create a ROS publisher for the objects
  pub_obj = nh.advertise<sensor_msgs::PointCloud2> ("pclobjects", 1);
 
  // Spin
  ros::spin ();
}

3. 실험
실행하면, 다음과 같이 세그먼테이션되고, 각 세그먼트의 중심점과 최대 영역점이 8 pixel의 점으로 표시된다.

4. 결론
특정 객체를 찾은 후에, 객체의 물성을 획득하면, 그 객체를 추적하거나, 그립하는 것이 가능하다. 이와 엑추에이터를 연계하기 위해서는 PID제어(앞의 글 참고)가 고려되어야 한다.
또한, moment_of_inertia_estimation.h와 같은 최신 버전의 API가 Indigo PCL버전에서는 제공되지 않는 것은 아쉬움이 있다.

유클리드 거리 기반 3차원 포인트 클라우드 세그먼테이션

이 글은 ROS PCL의 처리 속도를 고려해, 유클리드 거리 기반 3차원 포인트 클라우드 세그먼테이션을 처리해 보도록 한다.

1. 개요
유클리스 거리 기반 클러스터링 방법은 주어진 포인트 클라우드를 거리에 따라 그룹핑하는 효과적인 방법이다. 점 간 거리만 계산하고, 탐색하면 되므로, 처리속도가 다른 클러스터링 알고리즘에 비해 매우 빠르다. 다만, 세밀한 클러스터링을 할 수 없는 단점이 있다.

• euclid distance clustering

2. 코딩 
다음 프로그램은 계산 성능을 좀 더 높이기 위해, 복셀(voxel)을 이용해, LOD(level of detail)을 조정하여, 포인트 클라우드의 계산양을 줄인 후, 클러스터링한다. ROS에서 다음과 같이 코딩하고 빌드한다.

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

ros::Publisher pub;

typedef pcl::PointXYZ PointT;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);
 
  // Data containers used
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  vg.setInputCloud (cloud.makeShared());
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl;

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);
 

  std::cout << "Number of clusters is equal to " << cluster_indices.size () << std::endl;
 
  pcl::PointCloud<pcl::PointXYZI> TotalCloud; 
  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
        pcl::PointXYZ pt = cloud_filtered->points[*pit];
            pcl::PointXYZI pt2;
            pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
            pt2.intensity = (float)(j + 1);

            TotalCloud.push_back(pt2);
    }
    j++;
  }
  
    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(TotalCloud, cloud_p);
  
  sensor_msgs::PointCloud2 output; 
  pcl_conversions::fromPCL(cloud_p, output);
  output.header.frame_id = "camera_depth_optical_frame";   
  pub.publish(output); 
 
  ROS_INFO("published it."); 
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);

  // Create a ROS publisher for the output model coefficients
  // pub = nh.advertise<pcl_msgs::ModelCoefficients> ("pclplaneoutput", 1);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
 
  // Spin
  ros::spin ();
}


3. 실험
빌드 후 실행하면 다음과 같은 결과가 출력된다.

rviz에서 살펴보면, 다음과 같이 점 간 거리에 따라 적절히 그룹핑되었음을 확인할 수 있다.

실제 구분된 세그먼트까지 거리를 측정해 보자. XTion에서 파이프까지 거리는 대략 60cm이다 (줄자가 없어, 일단 자로 테스트한다).

rviz에서 axis를 추가한다. 축 길이는 비교하기 좋게 0.6 (60cm)로 한다.

그리고, measure명령으로 거리를 재어 보다. 아래와 같이 거의 비슷하게 거리값이 나오는 것을 확인할 수 있다.

각도도 대략 비교해 보았다. 큰 차이가 없어 보인다.

파이프 지름과 rviz상에 측정된 지름을 비교해 보았다. 파이프는 약 10cm지름을 가지므로, rviz측정 상에서 1.4 cm정도 오차가 있으며, 이는 파이프 반쪽면의 일부만 스캔된 점, LOD 처리로 포인트 밀도가 줄어든 점으로 인한 것이다. 

사다리를 놓고, 스캔을 해 보았다. 스캔된 가시영역과 그림자문제로 인해, 여러 개로 사다리가 세그먼테이션되는 것을 확인할 수 있다.

4. 결과 
이번에는 계산 성능을 고려해, 점 간 거리에 따른 클러스터링을 처리해 보았다. 성능은 다른 클러스터링 및 형상 추출 기법에 비해, 최소 20배 이상 빠르게 처리된다. 다만, 세밀한 단위로 클러스터링되지 않은 한계가 있다. 이는 앞의 글에서 설명한 다른 기법과 적절히 혼용하면 해결할 수 있다.

곡률 기반 형상 세그먼테이션

앞에서는 평면과 실린더를 기반으로 세그먼테이션을 처리해 보았다. 이 기법들은 몇몇 한계가 있는 데, 평면은 평면만 추출하는 문제, 실린더 추출은 실린더 내 세밀한 곡률의 변화를 구분하지 못하는 문제가 있다. 그러므로, 이 글에서는 곡률을 기반으로 형상을 분리하는 예를 통해, 이런 문제를 좀 더 개선해 보려 한다.

1. 개요
곡률 기반 세그먼테이션은 포인트의 법선 벡터 N을 이용해, 이웃점 N과의 곡률 C를 계산하고, 곡률의 차이를 이용해, 형상을 세그먼테이션 하는 기술이다. 각 점의 C를 계산하고, 유사도에 의해, 점을 그룹핑한다. 이는 유사 특성을 가진 점의 영역을 키워나가는 것이므로 region growing 알고리즘으로도 알려져 있다. 이 결과로 변곡점 부분을 분리해, 포인트 클라우드를 곡률 단위로 분해한다. 자세한 부분은 다음 내용을 참조한다.

• region growing segmentation

2. 코딩/빌드
다음과 같이 코딩하고 빌드한다.

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>

ros::Publisher pub;

typedef pcl::PointXYZ PointT;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);
 
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(cloud, cloud, indices);  
  std::cout << "cloud points = " << cloud.size () << std::endl;

  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
  pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setSearchMethod (tree);
  normal_estimator.setInputCloud (cloud.makeShared());
  normal_estimator.setKSearch (50);
  normal_estimator.compute (*normals);

  std::vector <pcl::PointIndices> clusters;
  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
  reg.setMinClusterSize (50);
  reg.setMaxClusterSize (1000000);
  reg.setSearchMethod (tree);
  reg.setNumberOfNeighbours (50);
  reg.setInputCloud (cloud.makeShared());
  reg.setInputNormals (normals);
  reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
  reg.setCurvatureThreshold (1.0);

  reg.extract (clusters);

  std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
  
    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(*colored_cloud, cloud_p);
  
  sensor_msgs::PointCloud2 output; 
  pcl_conversions::fromPCL(cloud_p, output);
  output.header.frame_id = "camera_depth_optical_frame";   
  pub.publish(output); 
 
  ROS_INFO("published it."); 
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);

  // Create a ROS publisher for the output model coefficients
  // pub = nh.advertise<pcl_msgs::ModelCoefficients> ("pclplaneoutput", 1);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
 
  // Spin
  ros::spin ();
}


3. 실험
rosrun으로 실행한다.

그리고 rviz를 통해 확인하면, 곡률에 따라 세그먼테이션이 되고 있는 모습을 볼 수 있다.

파라메터를 조정하면, 얼마나 민감하게 곡률에 따라 클러스터링할 것인지를 조정할 수 있다. 아래 파이프를 보면, 크게 6개 영역으로 구분된 것을 확인할 수 있다.

다음 실제 파이프와 비교해 보았을 때, 곡률 경계에 따라 어느정도 적절히 구분되었음을 확인할 수 있다.

4. 결과
곡률에 따라 포인트 클라우드를 분리하여, 클러스터링하는 region growing 알고리즘은 형상을 곡률을 기반으로 한 세밀한 단위로 분해할 수 있도록 한다. 다만, 법선, 곡률 계산 및 클러스터링 처리까지 포함되어 있어, 계산 속도가 늦다. 만약, 뭉쳐진 포인트 클라우드 단위로 클러스터링 한다면, 유클리드 거리 기반 클러스터링 알고리즘을 사용한 것이 낫다.

다중 실린더(Cylinder) 형상 추출

앞의 글에서는 다중 평면을 추출해 보았다. 이 글에서는 다중 실린더를 추출해 보도록 한다.

1. 개요
다중 실린더는 평면보다 처리할 것들이 더 많다. 실린더를 추출하기 전에 평면을 추출해, 제거해야 하며, 적절한 실린더 파라메터를 설정해 주어야 한다.

2. 코딩
다음과 같이 코딩해 본다. 추출할 반경은 0.05 미터에서 0.2 미터 반경 사이의 실린더만 추출해 본다 .빌드 및 실행 방법은 이전 글과 동일하다.

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>

ros::Publisher pub;

typedef pcl::PointXYZ PointT;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

    // remove plane
  for(int i = 0; i < 4; i++)
  {
      if(cloud.size() < 10000)
             break;
      seg.setInputCloud (cloud.makeShared ());
     
      pcl::ModelCoefficients coefficients;
      // pcl::PointIndices inliers;
      pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 
      seg.segment (*inliers, coefficients);
     
      // Create the filtering object
      pcl::ExtractIndices<pcl::PointXYZ> extract;
   
      // Extract the inliers
      ROS_INFO("extract the inliers"); 
      pcl::PointCloud<pcl::PointXYZ> in_cloud;
      extract.setInputCloud (cloud.makeShared ());
      extract.setIndices (inliers);
      extract.setNegative (false);
      extract.filter (in_cloud);
      extract.setNegative (true);
      extract.filter (cloud);
  }


  // Algorithm
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg2;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  // Datasets
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices);

  pcl::PointCloud<pcl::PointXYZI> TotalCloud;
  sensor_msgs::PointCloud2 output;
  for(int i = 0; i < 1; i++)
  {
        // Estimate point normals
        ne.setSearchMethod (tree);
        ne.setInputCloud (cloud.makeShared ());
        ne.setKSearch (50);
        ne.compute (*cloud_normals);

        // Create the segmentation object for cylinder segmentation and set all the parameters
        seg2.setOptimizeCoefficients (true);
        seg2.setModelType (pcl::SACMODEL_CYLINDER);
        seg2.setMethodType (pcl::SAC_RANSAC);
        seg2.setNormalDistanceWeight (0.1);
        seg2.setMaxIterations (1000);
        seg2.setDistanceThreshold (0.05);
        seg2.setRadiusLimits (0.05, 0.2);
        seg2.setInputCloud (cloud.makeShared ());
        seg2.setInputNormals (cloud_normals);

        // Obtain the cylinder inliers and coefficients
        seg2.segment (*inliers_cylinder, *coefficients_cylinder);
        std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

        // Write the cylinder inliers to disk
        pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
        pcl::ExtractIndices<PointT> extract;       
        extract.setInputCloud (cloud.makeShared ());
        extract.setIndices (inliers_cylinder);
        extract.setNegative (false);
        extract.filter (*cloud_cylinder);
       
      pcl::PointCloud<pcl::PointXYZ>::iterator index = cloud_cylinder->begin();
      for(; index != cloud_cylinder->end(); index++)
      {
         pcl::PointXYZ pt = *index;
         pcl::PointXYZI pt2;
         pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
         pt2.intensity = (float)(i + 1);

         TotalCloud.push_back(pt2);
      }
     
      ROS_INFO("%d. cylinder point cloud = %d", i, (int)TotalCloud.size()); 
  }
  
    // Convert To ROS data type 
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(TotalCloud, cloud_p); 
  pcl_conversions::fromPCL(cloud_p, output);
  output.header.frame_id = "camera_depth_optical_frame";   
  pub.publish(output); 
 
  ROS_INFO("published it."); 
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);

  // Create a ROS publisher for the output model coefficients
  // pub = nh.advertise<pcl_msgs::ModelCoefficients> ("pclplaneoutput", 1);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
 
  // Spin
  ros::spin ();
}

3. 실행 결과
실행 결과는 다음과 같다. PCL에서 7개의 파라메터가 추출되는 데, 앞의 3개 값은 실린더 축의 첫번째 좌표 P1, 다음 3개 값은 두번째 좌표 P2, 마지막 값은 반경이다. 0.18 반경을 가진 실린더가 추출되었음을 알 수 있다.

ROS rviz로 확인해 보면, 실린더와 유사한 형상이 추출되었음을 확인할 수 있다. 

4. 실험
이제 몇가지 실험을 해 본다. 실험을 위해 반경 0.05 m 인 파이프를 준비한다.

실행해 테스트해 보면, 다음과 같이 0.04 m 반경을 가진 파이프를 인식한 것을 확인할 수 있다. 인식 오차는 1 cm 정도이다.

rviz에서 확인해 보자. 우선 파이프를 추출하기 전 포인트 클라우드이다.

파이프를 인식해 보면, 앞의 포인트 클라우드에서 파이프 부분을 제대로 추출하였음을 확인할 수 있다. 다만, 일부 평면이 추출되었는 데, 이는 실린더 추출 시 설정 파라메터에서, 속도 문제로 반복횟수를 1000으로 낮추는 바람에, 허용오차가 제대로 고려되지 않은 문제로 보인다. 반경의 오차도 같은 이유로 높아진 원인이 있다. 물론, 불완전한 포인트 클라우드도 반경 오차에 영향을 미치고 있다. 

이제   for(int i = 0; i < 1; i++)  부분을 두번 실행되도록 2로 수정한다. 그리고 빌드해 실행해 본다. 그럼 포인트 클라우드에서 설정된 파라메터 조건에 맞는 추출된 2개의 실런더 형상을 확인할 수 있을 것이다 .

5. 결론
앞의 시험 결과, 처리 성능만 개선한다면, 파이프 파라메터와 피팅(fitting)율을 높일 수 있기 때문에, 파이프 추출 시 정밀도 등을 높일 수 있다. 그럼에도 불구하고, 평면에 비해, 실린더는 형상 인식에 더 많은 계산 시간이 필요하여, 실시간성이 요구되는 응용에는 적합하지 않을 수 있다.