PCL本家のexamplesをやります。
SIFTやります。
PCL本家のexamplesそのままではなくてRealSenseからの入力を使うように変更します。
RealSense + PCL
git cloneして、implして、buildして、runします。
準備
少し先の未来のことも考えながら整理します。
$ cd ~/develop
$ git clone https://github.com/PointCloudLibrary/pcl.git
$ mkdir 100_sift
$ (mv 001.cpp compile_test.cpp)
$ cp pcl/examples/keypoints/example_sift_keypoint_estimation.cpp 100_sift/sift.cpp
$ cd 100_sift
$ vi sift.cpp ../wrap/pcl-color/rs-pcl-color.cpp -O
impl
RealSenseのソースコードをPCLのSIFTへ移植したソースコードです。
移植のわかりやすさの為に、#includeの重複を許容しています。
インクルードガードで大丈夫のはずです。
PCLのexamplesの方でPCDからXYZRGBを取ってきている部分をコメントアウトして、RealSenseからの入力に置き換えています。
パラメーターの設定は「これだ !!」という設定にできているわけではありません。試している途中です。
sift.cpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/sift_keypoint.h>
// -----------------------------------------------------------------------------
#include <iostream>
#include <algorithm>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <string>
// Intel Realsense Headers
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
// PCL Headers
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>
#include <pcl/io/io.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
typedef pcl::PointXYZRGB RGB_Cloud;
typedef pcl::PointCloud<RGB_Cloud> point_cloud;
typedef point_cloud::Ptr cloud_pointer;
typedef point_cloud::Ptr prevCloud;
// -----------------------------------------------------------------------------
std::tuple<int, int, int> RGB_Texture(rs2::video_frame texture, rs2::texture_coordinate Texture_XY)
{
// Get Width and Height coordinates of texture
int width = texture.get_width(); // Frame width in pixels
int height = texture.get_height(); // Frame height in pixels
// Normals to Texture Coordinates conversion
int x_value = min(max(int(Texture_XY.u * width + .5f), 0), width - 1);
int y_value = min(max(int(Texture_XY.v * height + .5f), 0), height - 1);
int bytes = x_value * texture.get_bytes_per_pixel(); // Get # of bytes per pixel
int strides = y_value * texture.get_stride_in_bytes(); // Get line width in bytes
int Text_Index = (bytes + strides);
const auto New_Texture = reinterpret_cast<const uint8_t*>(texture.get_data());
// RGB components to save in tuple
int NT1 = New_Texture[Text_Index];
int NT2 = New_Texture[Text_Index + 1];
int NT3 = New_Texture[Text_Index + 2];
return std::tuple<int, int, int>(NT1, NT2, NT3);
}
// -----------------------------------------------------------------------------
cloud_pointer PCL_Conversion(const rs2::points& points, const rs2::video_frame& color){
// Object Declaration (Point Cloud)
cloud_pointer cloud(new point_cloud);
// Declare Tuple for RGB value Storage (<t0>, <t1>, <t2>)
std::tuple<uint8_t, uint8_t, uint8_t> RGB_Color;
//================================
// PCL Cloud Object Configuration
//================================
// Convert data captured from Realsense camera to Point Cloud
auto sp = points.get_profile().as<rs2::video_stream_profile>();
cloud->width = static_cast<uint32_t>( sp.width() );
cloud->height = static_cast<uint32_t>( sp.height() );
cloud->is_dense = false;
cloud->points.resize( points.size() );
auto Texture_Coord = points.get_texture_coordinates();
auto Vertex = points.get_vertices();
// Iterating through all points and setting XYZ coordinates
// and RGB values
for (int i = 0; i < points.size(); i++)
{
//===================================
// Mapping Depth Coordinates
// - Depth data stored as XYZ values
//===================================
cloud->points[i].x = Vertex[i].x;
cloud->points[i].y = Vertex[i].y;
cloud->points[i].z = Vertex[i].z;
// Obtain color texture for specific point
RGB_Color = RGB_Texture(color, Texture_Coord[i]);
// Mapping Color (BGR due to Camera Model)
cloud->points[i].r = get<2>(RGB_Color); // Reference tuple<2>
cloud->points[i].g = get<1>(RGB_Color); // Reference tuple<1>
cloud->points[i].b = get<0>(RGB_Color); // Reference tuple<0>
}
return cloud; // PCL RGB Point Cloud generated
}
// -----------------------------------------------------------------------------
int
main(int, char** argv)
{
// std::string filename = argv[1];
// std::cout << "Reading " << filename << std::endl;
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
rs2::pointcloud pc;
rs2::pipeline pipe;
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_COLOR);
cfg.enable_stream(RS2_STREAM_INFRARED);
cfg.enable_stream(RS2_STREAM_DEPTH);
rs2::pipeline_profile selection = pipe.start(cfg);
rs2::device selected_device = selection.get_device();
auto depth_sensor = selected_device.first<rs2::depth_sensor>();
if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
{
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter
pipe.wait_for_frames();
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0.f); // Disable emitter
}
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
// Query min and max values:
auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power
sleep(1);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, 0.f); // Disable laser
}
// Visualization of keypoints along with the original cloud
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
viewer.initCameraParameters(); // Camera Parameters for ease of viewing
for (;;)
{
auto frames = pipe.wait_for_frames();
auto depth = frames.get_depth_frame();
auto RGB = frames.get_color_frame();
pc.map_to(RGB);
auto points = pc.calculate(depth);
cloud_pointer cloud = PCL_Conversion(points, RGB);
// if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud) == -1) // load the file
// {
// PCL_ERROR ("Couldn't read file\n");
// return -1;
// }
std::cout << "points: " << cloud->size () <<std::endl;
// Parameters for sift computation
const float min_scale = 0.1f;
// const int n_octaves = 6;
const int n_octaves = 4;
// const int n_scales_per_octave = 10;
const int n_scales_per_octave = 5;
// const float min_contrast = 0.5f;
const float min_contrast = 0.2f;
// Estimate the sift interest points using Intensity values from RGB values
pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
pcl::PointCloud<pcl::PointWithScale> result;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
sift.setSearchMethod(tree);
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
sift.setMinimumContrast(min_contrast);
sift.setInputCloud(cloud);
sift.compute(result);
// Copying the pointwithscale to pointxyz so as visualize the cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr result_cloud (new pcl::PointCloud<pcl::PointXYZ>);
copyPointCloud(result, *result_cloud);
// Saving the resultant cloud
std::cout << "Resulting sift points are of size: " << result_cloud->size () <<std::endl;
pcl::io::savePCDFileASCII("sift_points.pcd", *result_cloud);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (result_cloud, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> cloud_color_handler (cloud, 255, 255, 0);
viewer.addPointCloud(cloud, "cloud");
viewer.addPointCloud(result_cloud, keypoints_color_handler, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
viewer.spinOnce();
viewer.removePointCloud ("cloud");
viewer.removePointCloud ("keypoints");
}
return 0;
}
build
Makefileを取ってきて、変更します。
LIBGLの行は無くてもOKと思いますが、削除はしません。
-I ../pcl/common/include -I ../pcl/surface/includeも要らないですね。
-l pcl_outofcore -l pcl_octreeも要らない。
$ cp ../wrap/pcl/pcl-color/Makefile .
$ vi Makefile
Makefile
CC = g++
CFLAGS = -O2
BIN = sift
OBJS = ${BIN}.o
INC = -I /usr/include/pcl-1.10 -I /usr/include/eigen3 -I /usr/include/vtk-7.1 -I ../pcl/common/include -I ../pcl/surface/include
LIB = -l realsense2 -l fw -l realsense-file -l usb-1.0 -l pthread -l udev -l boost_filesystem
LIBGL = -l GL -l glfw -l GLU
LIBPCL = -l pcl_common -l pcl_filters -l pcl_io -l pcl_visualization -l pcl_search -l pcl_outofcore -l pcl_octree
LIBVTK = -l vtkCommonCore-7.1 -l vtkCommonDataModel-7.1 -l vtkCommonMath-7.1 -l vtkRenderingCore-7.1
all : ${BIN}.o
${CC} ${CFLAGS} ${OBJS} ${LIB} ${LIBGL} ${LIBPCL} ${LIBVTK} -o ${BIN}
${BIN}.o : ${BIN}.cpp
${CC} ${CFLAGS} ${INC} -c $?
clean :
\rm sift sift.o
run
$ make
$ ./sift
これはsudoが不要でした。
きちんと特徴を把握できているかわからないですが、目的がRealSenseから入力を得てPCLに変換することなので、これでOKとします。
角は取得できているように見えます。
まとめ
これだけ情報を取れていれば色々とできそうです。
PCLのexamplesに見栄えのするのは少ないです。すぐにRealSenseとつなげられて見栄えがするのはSIFTくらい ?
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
