Kinect2
相信对这个话题感兴趣的同学, 对Kinect2应该也是很熟悉的吧。 这个设备现在也不贵, 某东上面大概两千左右就能买到, 并且还能配置一个三脚架。 Kinect2的效果, 确实会比1代要好很多, 无论是骨骼点还是图像质量等等。 更详细的介绍, 感兴趣的同学可以自行Google, 在MS官网上面查看。
ROS
做机器人相关的工作的同学, 同时对该部分也不会陌生吧。 机器人操作系统(ROS), 应用非常广泛, 并且有很多开源库, 包可以使用。 同时, 主流的传感器在ROS中也都有支持。 当然, Kinect2也是能够支持的。 ROS安装于Ubuntu之上, 我使用的版本是Ubuntu14.04 + ROS indigo。 关于ROS的安装问题, 可以查看官网相关指导 。 很简单的步骤。 依次输入下列命令:
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net --recv-key 0xB01FA116
sudo apt-get update
sudo apt-get install ros-indigo-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install Python-rosinstall
上述指令执行完毕之后, ROS也就安装完成了。 当然, 紧接着还需要建立自己的工作空间。 执行下述代码:
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
驱动节点配置
在ROS环境里使用Kinect2, 主要依靠iai-kinect2这个包。 Github地址: https://github.com/code-iai/iai_kinect2 。
首先, 需要安装其libfreenect2, 步骤如下(以下默认以Ubuntu14.04或更新的版本是系统版本, 别的系统, 参见原始网站说明):
ROS下Kinect2的驱动安装及简单应用PDF文档可以到Linux公社资源站下载:
——————————————分割线——————————————
免费下载地址在 http://linux.linuxidc.com/
用户名与密码都是www.linuxidc.com
具体下载目录在 /2017年资料/3月/19日/ROS下Kinect2的驱动安装及简单应用/
下载方法见 http://www.linuxidc.com/Linux/2013-07/87684.htm
——————————————分割线——————————————
libfreenect2
- 下载 libfreenect2 源码
git clone https://github.com/OpenKinect/libfreenect2.git
cd libfreenect2
- 下载upgrade deb 文件
cd depends; ./download_debs_trusty.sh
- 安装编译工具
sudo apt-get install build-essential cmake pkg-config
- 安装 libusb. 版本需求 >= 1.0.20.
sudo dpkg -i debs/libusb*deb
- 安装 TurboJPEG
sudo apt-get install libturbojpeg libjpeg-turbo8-dev
- 安装 OpenGL
sudo dpkg -i debs/libglfw3*deb
sudo apt-get install -f
sudo apt-get install libgl1-mesa-dri-lts-vivid
git clone https://github.com/OpenKinect/libfreenect2.git
cd libfreenect2
cd depends; ./download_debs_trusty.sh
sudo apt-get install build-essential cmake pkg-config
sudo dpkg -i debs/libusb*deb
sudo apt-get install libturbojpeg libjpeg-turbo8-dev
sudo dpkg -i debs/libglfw3*deb
sudo apt-get install -f
sudo apt-get install libgl1-mesa-dri-lts-vivid
(If the last command conflicts with other packages, don’t do it.) 原文如上提示, 我在安装的时候, 第三条指令确实出现了错误, 就直接忽略第三条指令了。
-
安装 OpenCL (可选)
-
Intel GPU:
sudo apt-add-repository ppa:floe/beignet sudo apt-get update sudo apt-get install beignet-dev sudo dpkg -i debs/ocl-icd*deb
-
AMD GPU:
apt-get install opencl-headers
- 验证安装:
clinfo
-
安装 CUDA (可选, Nvidia only):
- (Ubuntu 14.04 only) Download
cuda-repo-ubuntu1404...*.deb
(“deb (network)”) from Nvidia website, follow their installation instructions, includingapt-get install cuda
which installs Nvidia graphics driver. - (Jetson TK1) It is preloaded.
- (Nvidia/Intel dual GPUs) After
apt-get install cuda
, usesudo prime-select intel
to use Intel GPU for desktop. - (Other) Follow Nvidia website’s instructions.
- (Ubuntu 14.04 only) Download
- 安装 VAAPI (可选, Intel only)
sudo dpkg -i debs/{libva,i965}*deb; sudo apt-get install
- 安装 OpenNI2 (可选)
sudo apt-add-repository ppa:deb-rob/ros-trusty && sudo apt-get update
sudo apt-get install libopenni2-dev
- Build
cd ..
mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=$HOME/freenect2 -DENABLE_CXX11=ON
make
make install
针对上面cmake
命令的说明, 第一个参数, 是特别指定安装的位置, 你也可以指定别的你觉得高兴的地方, 但一般标准的路径是上述示例路径或者/usr/local
。 第二个参数是增加C++11的支持。
- 设定udev rules:
sudo cp ../platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/
, 然后重新插拔Kinect2. - 一切搞定, 现在可以尝试运行Demo程序:
./bin/Protonect
, 不出意外, 应该能够看到如下效果:
iai-kinect2
利用命令行从Github上面下载工程源码到工作空间内src文件夹内:
cd ~/catkin_ws/src/
git clone https://github.com/code-iai/iai_kinect2.git
cd iai_kinect2
rosdep install -r --from-paths .
cd ~/catkin_ws
catkin_make -DCMAKE_BUILD_TYPE="Release"
针对于上述命令中最后一行指令, 需要说明的是, 如果前面libfreenect2你安装的位置不是标准的两个路径下, 需要提供参数指定libfreenect2所在路径:
catkin_make -Dfreenect2_DIR=path_to_freenect2/lib/cmake/freenect2 -DCMAKE_BUILD_TYPE="Release"
编译结束, 一切OK的话, 会看到如下提示:
最后就是激动人心的时刻了, 在ROS中获取Kinect2的数据。
roslaunch kinect2_bridge kinect2_bridge.launch
使用roslaunch发起kinect2相关节点, 可以看到如下结果, 在另外一个命令行中, 输入rostopic list
, 可以查看到该节点发布出来的Topic, 还可以输入rosrun kinect2_viewer kinect2_viewer sd cloud
, 来开启一个Viewer查看数据。 结果如下图所示:
更多详情见请继续阅读下一页的精彩内容: http://www.linuxidc.com/Linux/2017-03/141955p2.htm
简单运用
很久没有留意这篇博客了, 上面的内容, 是之前一个工作中需要用到Kinect2, 所以试着弄了一下, 将其整理下来形成这篇博客的.
今天突然有一个同学在站内给我私信, 问我这篇博客的内容. 分享出来的东西帮助到了别人, 确实很高兴! 关于这位同学问到的问题, 其实在前面的工作中, 我也实现过. 下面试着回忆整理一下.
保存图片
其实目的就一个, 将Kinect2的RGB图保存下来. 在前面的叙述中, 输入rosrun kinect2_viewer kinect2_viewer sd cloud
来查看显示效果. 这句命令实质就是运行kinect2_viewer
包中的kinect2_viewer节点, 给定两个参数sd 和 cloud. 进入这个包的路径, 是可以看到这个节点源码. 源码中主函数摘录如下:
int main(int argc, char **argv) {
... ... // 省略了部分代码
topicColor = "/" + ns + topicColor;
topicDepth = "/" + ns + topicDepth;
OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR);
OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR);
// Receiver是一个类, 也定义在该文件中.useExact(true), useCompressed(false)
Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
OUT_INFO("starting receiver...");
// 运行时给出参数"cloud", 则mode = Receiver::CLOUD
// 运行时给出参数"image", 则mode = Receiver::IMAGE
// 运行时给出参数"both", 则mode = Receiver::BOTH
receiver.run(mode);
ros::shutdown();
return 0;
}
Receiver
类的实现也不算太复杂. 其中用于显示的主循环在imageViewer()
或cloudViewer()
中. 依据给的参数的不同, 将会调用不同的函数. 对应关系如下所示:
switch(mode) {
case CLOUD:
cloudViewer();
break;
case IMAGE:
imageViewer();
break;
case BOTH:
imageViewerThread = std::thread(&Receiver::imageViewer, this);
cloudViewer();
break;
}
BOTH选项, 将会出现两个窗口来显示图像. 上面两个函数都已经实现了图片保存的功能.代码摘录如下, 两个函数实现类似, 故只是摘录了imageViewer()
的内容:
int key = cv::waitKey(1);
switch(key & 0xFF) {
case 27:
case 'q':
running = false;
break;
case ' ':
case 's':
if(mode == IMAGE) {
createCloud(depth, color, cloud);
saveCloudAndImages(cloud, color, depth, depthDisp);
} else {
save = true;
}
break;
}
其中关键函数saveCloudAndImages()
的实现如下:
void saveCloudAndImages(
const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud,
const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored) {
oss.str("");
oss << "./" << std::setfill('0') << std::setw(4) << frame;
// 所有文件都保存在当前路径下
const std::string baseName = oss.str();
const std::string cloudName = baseName + "_cloud.pcd";
const std::string colorName = baseName + "_color.jpg";
const std::string depthName = baseName + "_depth.png";
const std::string depthColoredName = baseName + "_depth_colored.png";
OUT_INFO("saving cloud: " << cloudName);
// writer是该类的pcl::PCDWriter类型的成员变量
writer.writeBinary(cloudName, *cloud);
OUT_INFO("saving color: " << colorName);
cv::imwrite(colorName, color, params);
OUT_INFO("saving depth: " << depthName);
cv::imwrite(depthName, depth, params);
OUT_INFO("saving depth: " << depthColoredName);
cv::imwrite(depthColoredName, depthColored, params);
OUT_INFO("saving complete!");
++frame;
}
从上面代码中可以看出来, 如果想要保存图片, 只需要选中显示图片的窗口, 然后输入单击键盘s
键或者空格键就OK, 保存的图片就在当前目录.
如果有一些特别的需求, 在上面源码的基础上来进行实现, 将会事半功倍. 下面就是一个小小的例子.
保存图片序列
如果想要保存一个图片序列, 仅仅控制开始结束, 例如, 按键B(Begin)开始保存, 按键E(End)结束保存.
完成这样一个功能, 在源码的基础上进行适当更改就可以满足要求. 首选, 需要每一次对按键B和E的判断, 需要新增到上面摘录的switch(key & 0xFF)
块中. 需要连续保存的话, 简单的设定一个标志位即可.
首先, 复制vewer.cpp文件, 命名为save_seq.cpp. 修改save_seq.cpp文件, 在Receiver
类中bool save
变量下面添加一个新的成员变量, bool save_seq
. 在类的构造函数的初始化列表中新增该变量的初始化save_seq(false)
.
- 定位到
void imageViewer()
函数, 修改对应的switch(key & 0xFF)
块如下:
int key = cv::waitKey(1);
switch(key & 0xFF) {
case 27:
case 'q':
running = false;
break;
case 'b': save_seq = true; save = true; break;
case 'e': save_seq = false; save = false; break;
case ' ':
case 's':
if (save_seq) break;
if(mode == IMAGE) {
createCloud(depth, color, cloud);
saveCloudAndImages(cloud, color, depth, depthDisp);
} else {
save = true;
}
break;
}
if (save_seq) {
createCloud(depth, color, cloud);
saveCloudAndImages(cloud, color, depth, depthDisp);
}
- 定位到
void cloudViewer()
函数, 修改对应的if (save)
块如下:
if(save || save_seq) {
save = false;
cv::Mat depthDisp;
dispDepth(depth, depthDisp, 12000.0f);
saveCloudAndImages(cloud, color, depth, depthDisp);
}
- 定位到
void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)
函数, 修改源码如下:
if(event.keyUp()) {
switch(event.getKeyCode()) {
case 27:
case 'q':
running = false;
break;
case ' ':
case 's':
save = true;
break;
case 'b':
save_seq = true;
break;
case 'e':
save_seq = false;
break;
}
}
在CMakeList.txt的最后, 添加如下指令:
add_executable(save_seq src/save_seq.cpp)
target_link_libraries(save_seq
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${kinect2_bridge_LIBRARIES}
)
返回到catkin主目录, 运行catkin_make
, 编译, 链接没有问题的话, 就可以查看效果了. 当然了, 首先是需要启动kinect_bride的. 依次运行下述指令:
roslaunch kinect2_bridge kinect2_bridge.launch
rosrun kinect2_viewer save_seq hd cloud
选中弹出的窗口, 按键B 开始, 按键E结束保存. Terminal输出结果如下:
点击点云图获取坐标
主要想法是, 鼠标在点云图中移动时, 实时显示当前鼠标所指的点的三维坐标, 点击鼠标时, 获取该坐标发送出去.
这样的话, 首先就需要对鼠标有一个回调函数, 当鼠标状态改变时, 做出对应的变化. 鼠标变化可以简化为三种情况:
- 鼠标移动
- 鼠标左键点击
- 鼠标右键点击
因为涉及到回调函数, 而且也只是一个小功能, 代码实现很简单. 直接使用了三个全局变量代表这三个状态(代码需要支持C++11, 不想那么麻烦的话, 可以将类型更改为volatile int
), 以及一个全局的普通函数:
std::atomic_int mouseX;
std::atomic_int mouseY;
std::atomic_int mouseBtnType;
void onMouse(int event, int x, int y, int flags, void* ustc) {
// std::cout << "onMouse: " << x << " " << y << std::endl;
mouseX = x;
mouseY = y;
mouseBtnType = event;
}
在初始化中添加两个ros::Publisher
, 分别对应鼠标左键和右键压下应该发布数据到达的话题. 如下所示:
ros::Publisher leftBtnPointPub =
nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/left", 1);
ros::Publisher rightBtnPointPub =
nh.advertise<geometry_msgs::PointStamped>("/kinect2/click_point/right", 1);
其中消息格式是geometry_msgs/PointStamped
, ROS自带的消息, 在源码头部需要包含头文件, #include <geometry_msgs/PointStamped.h>
, 具体定义如下:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point point
float64 x
float64 y
float64 z
然后再重写cloudViewer()
函数如下:
void cloudViewer() {
cv::Mat color, depth;
std::chrono::time_point<std::chrono::high_resolution_clock> start, now;
double fps = 0;
size_t frameCount = 0;
std::ostringstream oss;
std::ostringstream ossXYZ; // 新增一个string流
const cv::Point pos(5, 15);
const cv::Scalar colorText = CV_RGB(255, 0, 0);
const double sizeText = 0.5;
const int lineText = 1;
const int font = cv::FONT_HERSHEY_SIMPLEX;
// 从全局变量获取当前鼠标坐标
int img_x = mouseX;
int img_y = mouseY;
geometry_msgs::PointStamped ptMsg;
ptMsg.header.frame_id = "kinect_link";
lock.lock();
color = this->color;
depth = this->depth;
updateCloud = false;
lock.unlock();
const std::string window_name = "color viewer";
cv::namedWindow(window_name);
// 注册鼠标回调函数, 第三个参数是C++11中的关键字, 若不支持C++11, 替换成NULL
cv::setMouseCallback(window_name, onMouse, nullptr);
createCloud(depth, color, cloud);
for(; running && ros::ok();) {
if(updateCloud) {
lock.lock();
color = this->color;
depth = this->depth;
updateCloud = false;
lock.unlock();
createCloud(depth, color, cloud);
img_x = mouseX;
img_y = mouseY;
const pcl::PointXYZRGBA& pt = cloud->points[img_y * depth.cols + img_x];
ptMsg.point.x = pt.x;
ptMsg.point.y = pt.y;
ptMsg.point.z = pt.z;
// 根据鼠标左键压下或右键压下, 分别发布三维坐标到不同的话题上去
switch (mouseBtnType) {
case cv::EVENT_LBUTTONUP:
ptMsg.header.stamp = ros::Time::now();
leftBtnPointPub.publish(ptMsg);
ros::spinOnce();
break;
case cv::EVENT_RBUTTONUP:
ptMsg.header.stamp = ros::Time::now();
rightBtnPointPub.publish(ptMsg);
ros::spinOnce();
break;
default:
break;
}
mouseBtnType = cv::EVENT_MOUSEMOVE;
++frameCount;
now = std::chrono::high_resolution_clock::now();
double elapsed =
std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;
if(elapsed >= 1.0) {
fps = frameCount / elapsed;
oss.str("");
oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)";
start = now;
frameCount = 0;
}
cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
ossXYZ.str("");
ossXYZ << "( " << ptMsg.point.x << ", " << ptMsg.point.y
<< ", " << ptMsg.point.z << " )";
cv::putText(color, ossXYZ.str(), cv::Point(img_x, img_y), font, 1, colorText, 3, CV_AA);
// cv::circle(color, cv::Point(mouseX, mouseY), 5, cv::Scalar(0, 0, 255), -1);
cv::imshow(window_name, color);
// cv::imshow(window_name, depth);
cv::waitKey(1);
}
}
cv::destroyAllWindows();
cv::waitKey(100);
}
最后, 稍微改写一下主函数就OK了, 整个主函数摘录如下, 其中去掉了多余的参数解析, 让使用更加固定, 简单.
int main(int argc, char **argv) {
#if EXTENDED_OUTPUT
ROSCONSOLE_AUTOINIT;
if(!getenv("ROSCONSOLE_FORMAT"))
{
ros::console::g_formatter.tokens_.clear();
ros::console::g_formatter.init("[${severity}] ${message}");
}
#endif
ros::init(argc, argv, "kinect2_viewer", ros::init_options::AnonymousName);
if(!ros::ok()) {
return 0;
}
std::string ns = K2_DEFAULT_NS;
std::string topicColor = K2_TOPIC_HD K2_TOPIC_IMAGE_COLOR K2_TOPIC_IMAGE_RECT;
std::string topicDepth = K2_TOPIC_HD K2_TOPIC_IMAGE_DEPTH K2_TOPIC_IMAGE_RECT;
bool useExact = true;
bool useCompressed = false;
Receiver::Mode mode = Receiver::CLOUD;
topicColor = "/" + ns + topicColor;
topicDepth = "/" + ns + topicDepth;
OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR);
OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR);
Receiver receiver(topicColor, topicDepth, useExact, useCompressed);
OUT_INFO("starting receiver...");
OUT_INFO("Please click mouse in color viewer...");
receiver.run(mode);
ros::shutdown();
return 0;
}
在CMakeList.txt
中加入下面两段话, 然后make
就OK.
add_executable(click_rgb src/click_rgb.cpp)
target_link_libraries(click_rgb
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
${kinect2_bridge_LIBRARIES}
)
install(TARGETS click_rgb
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
运行的话, 输入rosrun kinect2_viewer click_rgb
就OK. 效果如下图所示, 图中红色坐标位置, 实际是鼠标所在位置, 截图时, 鼠标截不下来.
本文永久更新链接地址:http://www.linuxidc.com/Linux/2017-03/141955.htm