数学公式网站
https://mathpix.com/
http://webdemo.myscript.com
1.安装ros插件
功能是创建msg文件时有提示
2. translation 安装翻译插件
3.hatchery插件
功能是创建launch 文件时有提示
3. CommonCode插件 辅助代码功能
3.CodeGlance插件
4. 配置默认使用xml打开urdf文件;
ps: 项目第一次创建urdf文件时就可以选择xml打开,但是你第一次选择的是txt文件默认打开的话需要在这个设置里把它改过来
4.1 删除默认txt文件打开
4.2添加默认xml文件方式打开urdf文件
4.3
4.4
ubuntu 把pip也换成国内源
mkdir ~/.pip cd ~/.pip touch pip.conf sudo nano ~/.pip/pip.conf打开pip.conf 文件后，添加以下内容:
[global] index-url = http://mirrors.aliyun.com/pypi/simple/ [install] trusted-host = pypi.tuna.tsinghua.edu.cn更新一下：
sudo apt-get update安装jupyter notebook
安装jupyter notebook conda install jupyter notebook 安装jupyter notebook的代码提示功能 pip install jupyter_contrib_nbextensions //报错就用下一行 pip install --user -i https://pypi.tuna.tsinghua.edu.cn/simple jupyter_contrib_nbextensions jupyter contrib nbextension install --user --skip-running-check 启动jupyter notebook jupyter notebook 点开 Nbextensions 的选项，并勾选 Hinterland
2.大小写匹配提示
3. 打开clion后不会进入上一次项目
4.opencv图片显示 ;目前只看到windows有这个功能
2. c++ class 模板配置
#parse("C File Header.h") #[[#ifndef]]# ${INCLUDE_GUARD} #[[#define]]# ${INCLUDE_GUARD} ${NAMESPACES_OPEN} class ${NAME} { public: ${NAME}(); ~${NAME}(); }; ${NAMESPACES_CLOSE} #[[#endif]]# //${INCLUDE_GUARD}#parse("C File Header.h") #[[#include]]# "${HEADER_FILENAME}" ${NAME}::${NAME}(){ } ${NAME}::~${NAME}(){ }
3.python ros环境配置
为了演示第一个程序，首先我们按照前面的例子，在workspace中创建一个demo_py的package，我们基于这个demo_py进行讲解开发。
使用clion打开demo_py。
在demo_py的目录中创建scripts目录，用于写python代码。
打开clion的setting，来到Build,Execution,Deployment的Python Interpreter下,点击设置按键，点击添加。
打开python环境设置按钮，添加python环境
Tip
特别要注意**的是，目前ROS Melodic版本还**不支持python3，要等到ROS N版才会支持。
因此，我们选择环境的时候**选择python2.x版本**。
注意: 新建的python文件需要改权限才能运行 chmoe 777 *
4.导入qt库
C++ 开发ROS项目过程中，如果要引入Qt，需要进行依赖的配置。
以新建的demo_turtlepackage为例，我们就要对CMakeLists.txt进行依赖配置。
Tip
默认的时候，这个配置是被注释起来的，我们只要解开注释就可以。
tip: find_package中，都是要加载的Qt模块，后续如果还需要添加其他的Qt模块，可以在后面追加。
来到CMakeLists.txt文件中，添加如下:
add_executable(turtle_control src/turtle_control.cpp) target_link_libraries(turtle_control ${catkin_LIBRARIES} Qt5::Core Qt5::Gui Qt5::Widgets Qt5::PrintSupport )add_executable是把turtle_control.cpp标记为可执行的
target_link_libraries是为turtle_control.cpp提供链接库的，值得注意的是，${catkin_LIBRARIES}是ros的依赖库，Qt5::Core，Qt5::Gui，Qt5::Wigets，Qt5::PrintSupport是qt的依赖库。
改变编译程序的存储路径，生成xml的保存路径
# 改变编译程序的存储路径，生成xml的保存路径 set(OUTPUT_DIRECTORY_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/build) # ${CMAKE_BUILD_TYPE} set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${OUTPUT_DIRECTORY_ROOT}/bin" CACHE PATH "Runtime directory" FORCE) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${OUTPUT_DIRECTORY_ROOT}/lib" CACHE PATH "Library directory" FORCE) set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${OUTPUT_DIRECTORY_ROOT}/lib" CACHE PATH "Archive directory" FORCE)catkit_make_source.sh
用 source catkit_make_source.sh 命令启动
#!/bin/bash catkin_make if [[ $? != 0 ]]; then echo 'catkin_make :error' else echo 'catkin_make :ok' source devel/setup.bash echo 'source devel/setup.bash :ok' ficlion_start.sh
#!/bin/bash bash ~/CLion-2020.1.1/clion-2020.1.1/bin/clion.shinWorkspaceWorkProject.sh
用控制台命令: source inWorkspaceWorkProject.sh 启动
或者 . ./inWorkspaceWorkProject.sh 命令运行脚本,第二个命令注意两个点之间是有空格的;
#!/bin/bash cd ~/workspace/ros_work5
receiver.sh
#!/bin/bash bash ~/receiver/start.sh 192.168.22.255http://robot.czxy.com/car/
查看摄像头 ll /dev/video* 查询 c++ 库中的方法函数 进opencv_env/opencv-3.4.9 目录下 grep -rniI "cornerHarris(" * grep -rniI "cornerHarris(" * --include=*.cpp 制作 命令台命令： 通过一个命令启动 例如： 在 ~/.bashrc 中加入 alias sd='source devel/setup.sh' 在命令台中就可以使用 sd 命令
断开拨号上网
sudo poff dsl-provider sudo dhclientsudo apt install ros-melodic-joint-state-publisher-gui
编译工具
sudo apt-get install build-essentia摄像头端口
/dev/video*
模板数据
py_ros_main_node节点
#!/usr/bin/env python # coding:utf-8 import rospy # 导入数据 from std_msgs.msg import String if __name__ == '__main__': # 节点名 nodeName = 'py_publisher' # 初始化节点 定义匿名节点加参数 anonymous=True rospy.init_node(nodeName,anonymous=True)py_ros_node初始化
import rospy # 节点名 nodeName = 'py_publisher' # 初始化节点 定义匿名节点加参数 anonymous=True rospy.init_node(nodeName,anonymous=True)
1.py_ros_class_PyQt5
#!/usr/bin/env python # coding:utf-8 from PyQt5.QtWidgets import QWidget,QFormLayout,QLineEdit,QPushButton,QLabel from geometry_msgs.msg import Twist from turtlesim.msg import Pose import rospy class MainWindow(QWidget): def __init__(self,publisher): super(MainWindow, self).__init__() pass2.py_ros_main_PyQt5_node初始化
#!/usr/bin/env python # coding:utf-8 from PyQt5.QtWidgets import QApplication from MainWindow import MainWindow import sys import rospy if __name__ == '__main__': # 创建节点 rospy.init_node("py_control",anonymous=True) # 创建Qt app = QApplication(sys.argv) # 创建窗口 w = MainWindow() # 展示窗口 w.show() # 等待程序结束 sys.exit(app.exec_())3.1server
py_ros_client_main通讯
#!/usr/bin/env python # coding:utf-8 import rospy from roscpp_tutorials.srv import TwoInts,TwoIntsRequest if __name__ == '__main__': # 创建节点 rospy.init_node("py_client",anonymous=True) # service名字 serviceName = 'py_service' # 创建客户端 proxy = rospy.ServiceProxy(serviceName, TwoInts) # 等待service上线 proxy.wait_for_service() # 请求数据 req = TwoIntsRequest() req.a = 12 req.b = 13 # 调用service result = proxy.call(req) print resultpy_ros_client通讯
import rospy from roscpp_tutorials.srv import TwoInts,TwoIntsRequest # service名字 serviceName = 'py_service' # 创建客户端 proxy = rospy.ServiceProxy(serviceName, TwoInts) # 等待service上线 proxy.wait_for_service() # 请求数据 req = TwoIntsRequest() req.a = 12 req.b = 13 # 调用service result = proxy.call(req) print result
3.1 py_ros_server_main通讯
#!/usr/bin/env python # coding:utf-8 import rospy from roscpp_tutorials.srv import TwoInts def callBack(req): return req.a+req.b if __name__ == '__main__': # 创建节点 rospy.init_node("py_server",anonymous=True) # service名字 serviceName = 'py_service' # 创建server端 service = rospy.Service(serviceName, TwoInts, callBack) # 阻塞 rospy.spin()py_ros_server通讯
import rospy from roscpp_tutorials.srv import TwoInts def callBack(req): return req.a+req.b # 创建节点 rospy.init_node("py_server",anonymous=True) # service名字 serviceName = 'py_service' # 创建server端 service = rospy.Service(serviceName, TwoInts, callBack) # 阻塞 rospy.spin()
4. topic
py_ros_publisher_main_topic通讯
#!/usr/bin/env python # coding:utf-8 import rospy # 导入数据 from std_msgs.msg import String if __name__ == '__main__': # 节点名 nodeName = 'py_publisher' # 初始化节点 rospy.init_node(nodeName,anonymous=True) # 话题名 topicName = 'py_topic' # 创建发布者 publisher = rospy.Publisher(topicName, String, queue_size=5) # 每一秒钟发送一条消息 rate = rospy.Rate(1) i = 0 # 循环 while not rospy.is_shutdown(): # 消息内容 data = String() data.data = 'hello {}'.format(i) # 发送消息 publisher.publish(data) # 增加数据 i += 1 rate.sleep()
py_ros_publisher通讯
import rospy # 导入数据 from std_msgs.msg import String # 话题名 topicName = 'py_topic' # 创建发布者 publisher = rospy.Publisher(topicName, String, queue_size=5) # 每一秒钟发送一条消息 rate = rospy.Rate(1) i = 0 # 循环 while not rospy.is_shutdown(): # 消息内容 data = String() data.data = 'hello {}'.format(i) # 发送消息 publisher.publish(data) # 增加数据 i += 1 rate.sleep()
py_ros_subscriber_main通讯
#!/usr/bin/env python # coding:utf-8 import rospy # from std_msgs.msg import String from geometry_msgs.msg import Twist import threading # python订阅者回调在子线程中 def callBack(data): print '接收的线程id:{}'.format(threading.current_thread().name) print '收到数据:{}'.format(data.linear.x) if __name__ == '__main__': print '主线程线程id:{}'.format(threading.current_thread().name) # 节点名 nodeName = 'py_subscriber' # 初始化节点 rospy.init_node(nodeName,anonymous=True) # topic名字 topicName = 'py_topic' # 创建订阅者 # 参数1：topic名字 # 参数2：topic数据类型 # 参数3：回调函数 subscriber = rospy.Subscriber(topicName, Twist, callBack) # 事件处理 rospy.spin()
py_ros_subscriber通讯
import rospy # from std_msgs.msg import String from geometry_msgs.msg import Twist def callBack(data): data.linear.x print '收到数据:{}'.format(data.linear.x) # topic名字 topicName = 'py_topic' # 创建订阅者 # 参数1：topic名字 # 参数2：topic数据类型 # 参数3：回调函数 subscriber = rospy.Subscriber(topicName, Twist, callBack) # 事件处理 rospy.spin()
py_time
#!/usr/bin/env python # coding:utf-8 import rospy from PyQt5.QtWidgets import * from PyQt5.QtCore import QTimer class MainWindow(QWidget): def __init__(self): super(MainWindow, self).__init__() self.timer = QTimer(self) self.timer.setInterval(100) self.timer.timeout.connect(self.timeout) self.timer.start() def timeout(self): #定时的槽函数 判断ros是否关闭,ros关闭后关闭窗口 if rospy.is_shutdown(): QApplication.quit()py_getThread_Id获取多线程的线程ID
import threading print '接收的线程id:{}'.format(threading.current_thread().name)
4.action标准通讯
py_ros_action_client_main标准通讯
#!/usr/bin/env python # coding:utf-8 import rospy from actionlib import ActionClient,ClientGoalHandle,CommState,TerminalState from demo_actions.msg import CountNumberAction,CountNumberGoal def transition_cb(goalHandle): commState = goalHandle.get_comm_state() if commState==CommState.PENDING: rospy.loginfo('PENDING') elif commState==CommState.ACTIVE: rospy.loginfo('ACTIVE') elif commState==CommState.DONE: rospy.loginfo('DONE') def feedback_cb(goalHandle,feedback): rospy.loginfo('feedback') if __name__ == '__main__': # 创建节点 rospy.init_node("action_client") # action名字 actionName = 'action_py' # 创建客户端 client = ActionClient(actionName,CountNumberAction) # 等待 client.wait_for_server() goal = CountNumberGoal() goal.max = 10 goal.duration = 1 # 发送指令 goal = client.send_goal(goal,transition_cb,feedback_cb) # 阻塞 rospy.spin()py_ros_action_server_main标准通讯
#!/usr/bin/env python # coding:utf-8 import rospy from actionlib import ActionServer,ServerGoalHandle from demo_actions.msg import CountNumberAction,CountNumberFeedback def goal_cb(goalHandle): rospy.loginfo('receive goal') # goal = ServerGoalHandle() # 接受 goalHandle.set_accepted() # 获取目标 max = goalHandle.get_goal().max duration = goalHandle.get_goal().duration for i in range(1,max): feed = CountNumberFeedback() feed.percent = float(i)/max # 进度 goalHandle.publish_feedback(feed) def cancel_cb(goalHandle): rospy.loginfo('cancel goal') if __name__ == '__main__': # 创建节点 rospy.init_node("action_server") # action名字 actionName = 'action_py' # server server = ActionServer(actionName,CountNumberAction,goal_cb,cancel_cb,auto_start=False) # 开启服务 server.start() # 阻塞 rospy.spin()py_ros_action_client_class标准通讯
#! /usr/bin/env python # coding: utf-8 from PyQt5.QtWidgets import QWidget,QFormLayout,QLineEdit,QLabel,QPushButton from PyQt5.QtCore import QTimer from actionlib import ActionClient,ClientGoalHandle,CommState,TerminalState from demo_actions.msg import CountNumberAction,CountNumberGoal,CountNumberResult,CountNumberFeedback import rospy class ActionClientWindow(QWidget): def __init__(self): super(ActionClientWindow, self).__init__() # 自定义 timer， 负责页面更新 # update_timer = QTimer(self) # update_timer.setInterval(16) # update_timer.start() # # timer事件回调 # update_timer.timeout.connect(self.on_update) # 设置title self.setWindowTitle("") layout = QFormLayout() self.setLayout(layout) self.le_max = QLineEdit("100") layout.addRow("数到多少", self.le_max) self.le_duration = QLineEdit("0.1") layout.addRow("间隔时间", self.le_duration) self.lb_percent = QLabel() layout.addRow("进度显示", self.lb_percent) self.lb_result = QLabel() layout.addRow("结果显示", self.lb_result) self.lb_result_state = QLabel() layout.addRow("结果状态", self.lb_result_state) btn_send = QPushButton("发送") layout.addRow(btn_send) btn_cancel = QPushButton("取消执行") layout.addRow(btn_cancel) btn_send.clicked.connect(self.click_send) btn_cancel.clicked.connect(self.click_cancel) # 创建Action client action_name = "/hello/action" self.client = ActionClient(action_name, CountNumberAction) self.handle = None def click_send(self): goal = CountNumberGoal() goal.max = long(self.le_max.text()) goal.duration = float(self.le_duration.text()) self.handle = self.client.send_goal(goal, self.transition_cb, self.feedback_cb) def transition_cb(self, handle): if not isinstance(handle, ClientGoalHandle): return comm_state = handle.get_comm_state() if comm_state == CommState.ACTIVE: print "ACTIVE" elif comm_state == CommState.DONE: # 干完活后的回调 state = handle.get_terminal_state() if state == TerminalState.ABORTED: # sever中断 self.lb_result_state.setText("sever中断") elif state == TerminalState.PREEMPTED: # client取消 self.lb_result_state.setText("client取消") elif state == TerminalState.REJECTED: # client数据校验未通过 self.lb_result_state.setText("client数据校验未通过") elif state == TerminalState.SUCCEEDED: # SUCCEEDED self.lb_result_state.setText("SUCCEEDED") result = handle.get_result() if not isinstance(result, CountNumberResult): return self.lb_result.setText(str(result.count)) def feedback_cb(self, handle, feedback): if not isinstance(handle, ClientGoalHandle): return if not isinstance(feedback, CountNumberFeedback): return self.lb_percent.setText(str(feedback.percent)) def click_cancel(self): print "cancel" if not isinstance(self.handle, ClientGoalHandle): return self.handle.cancel() # def on_update(self): # # qt刷新 # self.update() # # # 判断用户终止操作 # if rospy.is_shutdown(): # self.close()py_ros_action_server_class标准通讯
#! /usr/bin/env python # coding: utf-8 from PyQt5.QtWidgets import QWidget, QTableView, QFormLayout, QPushButton, QLabel, QLineEdit from PyQt5.QtGui import QStandardItemModel,QStandardItem from PyQt5.QtCore import QTimer,pyqtSignal,Qt from actionlib import ActionServer,ServerGoalHandle from demo_actions.msg import CountNumberAction,CountNumberGoal,CountNumberFeedback,CountNumberResult import rospy import threading class ActionServerWindow(QWidget): # 创建信号 _update_signal = pyqtSignal() def __init__(self): super(ActionServerWindow, self).__init__() print threading.current_thread().name # 自定义 timer， 负责页面更新 update_timer = QTimer(self) update_timer.setInterval(16) update_timer.start() # timer事件回调 update_timer.timeout.connect(self.on_update) # 设置title self.setWindowTitle("") layout = QFormLayout() self.setLayout(layout) self.table = QTableView() layout.addRow(self.table) self.model = QStandardItemModel() self.table.setModel(self.model) # 为表格设计两个列 self.model.setColumnCount(2) self.model.setHeaderData(0, Qt.Horizontal, "任务ID") self.model.setHeaderData(1, Qt.Horizontal, "终止操作") # 存放用户操作的数据 self.map = {} # 绑定事件信号 # btn = QPushButton("") # btn.clicked.connect(self.click) self._update_signal.connect(self.update_table) # 新建Action Server action_name = "/hello/action" self.server = ActionServer(action_name, CountNumberAction, self.goal_cb, self.cancel_cb, False) self.server.start() def goal_cb(self, handle): print threading.current_thread().name if not isinstance(handle, ServerGoalHandle): return # UI上需要显示 任务 id = handle.get_goal_id().id self.map[id] = { "id": id, "isCanceled": False, "isAborted": False } # 在子线程中发送信号，调用ui更新 self._update_signal.emit() # self.update_table() # 开启子线程，去执行任务 thread = threading.Thread(target=self.do_goal, args=(handle,)) thread.start() def update_table(self): # model清理 self.model.clear() row = 0 # 循环便利数据，将数据显示到table上 for key in self.map.keys(): value = self.map[key] self.model.setItem(row, 0, QStandardItem(value["id"])) self.model.setItem(row, 1, QStandardItem("xxxxxxxxxxxxxx")) btn = QPushButton("终止操作") btn.clicked.connect(lambda: self.do_aborted(value["id"])) self.table.setIndexWidget(self.model.index(row, 1), btn) row += 1 def do_aborted(self, id): print "do_aborted: {}".format(id) # 修改记录的数据 self.map[id]["isAborted"] = True def do_goal(self, handle): if not isinstance(handle, ServerGoalHandle): return # server接收到client goal指令 id = handle.get_goal_id().id goal = handle.get_goal() if not isinstance(goal, CountNumberGoal): # 数据不符合要求 handle.set_rejected() return # 1. 数据校验 # 2. 完成业务 # 3. 发布进度 # 4. 发布结果 # 5. 取消操作 # 6. 中断操作 # 1. 数据校验 max = goal.max duration = goal.duration if max < 0 or duration < 0: handle.set_rejected() return # 数据校验成功后，手动更改状态为 Active handle.set_accepted() count = 0 while not rospy.is_shutdown() and count < max: # 5. 取消操作: if self.map[id]["isCanceled"]: break # 6. 中断操作 if self.map[id]["isAborted"]: break # 3. 发布进度 feedback = CountNumberFeedback() feedback.percent = count * 100.0 / max handle.publish_feedback(feedback) # print feedback.percent # 2. 完成业务 count += 1 rospy.sleep(duration) # 4. 发布结果 result = CountNumberResult() result.count = count if self.map[id]["isAborted"]: handle.set_aborted(result) elif self.map[id]["isCanceled"]: handle.set_canceled(result) else: handle.set_succeeded(result) # 清理数据 del self.map[id] # 更新UI self._update_signal.emit() def cancel_cb(self, handle): if not isinstance(handle, ServerGoalHandle): return # 接收到取消的逻辑 id = handle.get_goal_id().id # 设置数据 self.map[id]["isCanceled"] = True def on_update(self): # qt刷新 self.update() # 判断用户终止操作 if rospy.is_shutdown(): self.close()py_param_get_param_names获取传的参数
result = rospy.get_param_names() for ele in result: value = rospy.get_param(ele) rospy.loginfo(value)py_qt+pyqtSignal自定义信号与曹
from PyQt5.QtCore import pyqtSignal # 自定义信号 全局变量 updateUI = pyqtSignal() # 绑定自定义信号 定义在构造方法中 self.updateUI.connect(self.handle) # 发送信号 self.updateUI.emit() def handle(self): print '收到信号'py_qt_connect_class绑定信号和槽
# 绑定信号和槽 self.btn.clicked.connect(self.click) def click(self): print '收到信号'py_thread_class开启线程
import threading # 开启子线程，去执行任务 thread = threading.Thread(target=self.do_goal, args=(handle,)) thread.start() def do_goal(self, handle): passpy_Gripper_因时电动夹爪
# 导入串口库 from serial import Serial class Gripper: def __init__(self,port): self.serial = Serial(port, 115200) def catch(self,speed=500,power=100): # 创建串口通讯类 # 参数1：串口端口 /dev/ttyUSB0 # 0xEB 0x90 # 0x01 # 0x05 # 0x10 # 0xF4 0x01 0x64 0x00 # 校验和 # B9 # (1 Byte) # 0x6F b0b1 = b'\xEB\x90' b2 = b'\x01' b3 = b'\x05' b4 = b'\x10' # 获取低位 b5 = bytearray([speed&0x00ff]) b6 = bytearray([speed>>8]) b7 = bytearray([power&0x00ff]) b8 = bytearray([power>>8]) # 把b2+b8 取低字节 b9 = bytearray([(ord(b2)+ord(b3)+ord(b4)+ord(b5)+ord(b6)+ord(b7)+ord(b8))&0x00ff]) data = b0b1+b2+b3+b4+b5+b6+b7+b8+b9 # 写入串口数据 字节数组 self.serial.write(data) def release(self,speed=500): # 0xEB 0x90 # 0x01 # 数据体长度 # B3 # (1 Byte) # 0x03 # 指令号 # B4 # (1 Byte) # 0x11 # 数据内容 # B5B6 # (1 Byte) # 0xF4 0x01 # 校验和 # B7 # (1 Byte) # 0x0A b0b1 = b'\xEB\x90' b2 = b'\x01' b3 = b'\x03' b4 = b'\x11' b5 = bytearray([speed & 0x00ff]) b6 = bytearray([speed >> 8]) b7 = bytearray([(ord(b2)+ord(b3)+ord(b4)+ord(b5)+ord(b6))&0x00ff]) data = b0b1 + b2 + b3 + b4 + b5 + b6 + b7 # 写入串口数据 字节数组 self.serial.write(data) def close(self): self.serial.close() gripper = Gripper('/dev/ttyUSB0') # gripper.catch(speed=100,power=80) gripper.release(speed=100) # catch() # release()平移公式
py_ros_平移公式
import numpy as np # 已知的值 # p点在b坐标系中的位置 pb = (3, 4) # b坐标系的原点在a坐标系中的位置 b = (3, 2) # 求救 p 在a坐标系中的位置 matrix = np.array([ #交换矩阵 [1, 0, b[0]], [0, 1, b[1]], [0, 0, 1], ]) point = np.array([ [pb[0]], [pb[1]], [1] ]) pa = matrix.dot(point) #点乘旋转公式
py_ros_旋转公式
import numpy as np from math import sqrt, sin, cos, radians # p在b坐标系中的位置 pb = (12, 2) # b坐标系相对于a坐标系旋转的角度 theta = radians(30) # 构建变换矩阵 matrix = np.array([ [cos(theta), -sin(theta)], [sin(theta), cos(theta)] ]) point = np.array([ [pb[0]], [pb[1]] ]) # 求解 pa = matrix.dot(point)py_ros_旋转加平移公式
import numpy as np from math import sqrt, sin, cos, radians # p点在b坐标系中的位置 pb = (sqrt(12), 2) # b坐标系相对于a坐标系的移动 delta = (3, 2) # b坐标系相对于a坐标系的旋转 theta = radians(30) # 变换矩阵 matrix = np.array([ [cos(theta), -sin(theta), delta[0]], [sin(theta), cos(theta), delta[1]], [0, 0, 1], ]) point = np.array([ [pb[0]], [pb[1]], [1] ]) # p在a中的表达 pa = matrix.dot(point)py_ur3_aubo5_移动
from moveit_commander import MoveGroupCommander from geometry_msgs.msg import Pose from tf import transformations from math import radians commander = MoveGroupCommander("manipulator_i5") commander = MoveGroupCommander("manipulator") # commander.set_named_target("zero") pose = Pose() pose.position.x = -0.282259 pose.position.y = -0.444101 pose.position.z = 0.580849 + 0.502 # 欧拉角转四元素 quat = transformations.quaternion_from_euler(radians(129.411392), radians(15.759084), radians(-33.136116)) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] commander.set_pose_target(pose) # success = commander.go() if success: rospy.loginfo("success") else: rospy.loginfo("failed")#from moviepy.editor import VideoFileClip
py_opencv_移动_warpAffine
import cv2 as cv import numpy as np img = cv.imread("img/123.jpg" ,cv.IMREAD_COLOR) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 定义位移矩阵 x移动50 y移动100 matrixShift = np.float32([ [1,0,50], [0,1,100] ]) # 调用api dstImg = cv.warpAffine(img,matrixShift,(width,height)) cv.imshow("dst",dstImg) cv.waitKey(0) cv2.destroyAllWindows()py_opencv_缩放_resize
import cv2 as cv # 读取一张图片 img = cv.imread("img/123.jpg" , cv.IMREAD_COLOR) # 获取图片信息 imgInfo = img.shape print(imgInfo) # 获取图片的高度 height = imgInfo[0] # 获取图片的宽度 width = imgInfo[1] # 获取图片的颜色模式,表示每个像素点由3个值组成 mode = imgInfo[2] # 定义缩放比例 newHeight = int(height*0.5) newWidth = int(width*0.5) # 使用api缩放 newImg = cv.resize(img, (newWidth, newHeight)) # 将图片展示出来 cv.imshow("result",newImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_旋转_warpAffine
img = cv.imread("img/123.jpg" ,cv.IMREAD_COLOR) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 定义仿射矩阵： 参数1：中心点， 参数2：旋转角度，参数3：缩放系数 matrixAffine = cv.getRotationMatrix2D((width * 0.5, height * 0.5), 45, 0.5) # 进行仿射变换 dstImg = cv.warpAffine(img, matrixAffine, (width, height)) cv.imshow("dstImg",dstImg) cv.waitKey(0) cv2.destroyAllWindows()py_opencv_镜像
import cv2 as cv import numpy as np img = cv.imread("img/123.jpg", cv.IMREAD_COLOR) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 创建一个两倍于原图大小的矩阵 dstImg = np.zeros((height*2,width,3),np.uint8) # 向目标矩阵中填值 for row in range(height): for col in range(width): # 上部分直接原样填充 dstImg[row,col] = img[row,col] # 下半部分倒序填充 dstImg[height*2-row-1,col] = img[row,col] # 显示图片出来 cv.imshow("dstImg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_剪切
import cv2 as cv # 读取原图 img = cv.imread("img/lena.jpg",cv.IMREAD_COLOR) cv.imshow("source",img) # 从图片（230,230） 截取一张 宽度130，高度70的图片 dstImg = img[180:250,180:310] # 显示图片 cv.imshow("result",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_拉伸_warpAffine
import cv2 as cv import numpy as np # 读取一张图片 img = cv.imread("img/123.jpg" , cv.IMREAD_COLOR) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 定义图片 左上角，左下角 右上角的坐标 matrixSrc = np.float32([[0,0],[0,height-1],[width-1,0]]) # 将原来的点映射到新的点 matrixDst = np.float32([[50,100],[300,height-200],[width-300,100]]) # 将两个矩阵组合在一起,仿射变换矩阵 matrixAffine = cv.getAffineTransform(matrixSrc,matrixDst) dstImg = cv.warpAffine(img,matrixAffine,(width,height)) cv.imshow("dstImg", dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_金子塔_pyrDownUp
import cv2 as cv import numpy as np # 读取一张图片 src_img = cv.imread("img/123.jpg" , cv.IMREAD_COLOR) imgInfo = src_img.shape height = imgInfo[0] width = imgInfo[1] pry_down1 = cv.pyrDown(src_img) cv.imshow("down1",pry_down1) pry_down2 = cv.pyrDown(pry_down1) cv.imshow("down2",pry_down2) pry_down3 = cv.pyrDown(pry_down2) cv.imshow("down3",pry_down3) pry_down4 = cv.pyrDown(pry_down3) cv.imshow("down4",pry_down4) pyr_up1 = cv.pyrUp(pry_down1) cv.imshow("up1",pyr_up1) pyr_up2 = cv.pyrUp(pry_down2) cv.imshow("up2",pyr_up2) pyr_up3 = cv.pyrUp(pry_down3) cv.imshow("up3",pyr_up3) pyr_up4 = cv.pyrUp(pry_down4) cv.imshow("up4",pyr_up4) # 对比resize img2 = cv.resize(src_img,(int(height/2),int(width/2))) cv.imshow("img1/2",img2) img4 = cv.resize(src_img,(int(height/4),int(width/4))) cv.imshow("img1/4",img4) img8 = cv.resize(src_img,(int(height/8),int(width/8))) cv.imshow("img1/8",img8) img16 = cv.resize(src_img,(int(height/16),int(width/16))) cv.imshow("img1/16",img16) cv.waitKey(0) cv.destroyAllWindows()py_opencv_图像融合_addWeighted
import cv2 as cv import numpy as np # 读取一张图片 两张图片尺寸需要一致 itheima = cv.imread("img/123.jpg", cv.IMREAD_COLOR) cv.imshow("itheima",itheima) tony = cv.imread("img/333.jpg", cv.IMREAD_COLOR) cv.imshow("tony",tony) # 进行叠加时的插值 dst = cv.addWeighted(itheima,0.5,tony,0.5,0) cv.imshow("dst",dst) cv.waitKey(0) cv.destroyAllWindows()py_opencv_灰度处理_cvtColor
import cv2 as cv # 方式一 ： 直接以灰度图像的形式读取 # img = cv.imread("img/123.jpg", cv.IMREAD_GRAYSCALE) # cv.imshow("dstImg",img) # cv.waitKey(0) # 方式二： 以彩图的方式读取 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) # 将原图的所有颜色转成灰色 dstImg = cv.cvtColor(img, cv.COLOR_BGR2GRAY) cv.imshow("dstImg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_灰图反转
import cv2 as cv import numpy as np # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_GRAYSCALE) cv.imshow("img",img) # 获取原图信息 imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 创建一个和原图同样大小的矩阵 dstImg = np.zeros((height,width,1),np.uint8) for row in range(height): for col in range(width): # 获取原图中的灰度值 gray = img[row,col] # 反转 newColor = 255 - gray # 填充 dstImg[row,col]=newColor cv.imshow("dstimg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_彩图反转
import cv2 as cv import numpy as np # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) # 获取原图信息 imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 创建一个和原图同样大小的矩阵 dstImg = np.zeros((height,width,3),np.uint8) for row in range(height): for col in range(width): # 获取原图中的灰度值 (b,g,r) = img[row,col] # 反转 new_b = 255-b new_g = 255-g new_r = 255-r # 填充 dstImg[row,col]=(new_b,new_g,new_r) cv.imshow("dstimg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_马赛克效果
import cv2 as cv import numpy as np # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) # 获取原图信息 imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 遍历要打马赛克的区域 宽度430 高度220 for row in range(160,240): for col in range(380,670): # 每10×10的区域将像素点颜色改成一致 if row%10==0 and col%10==0: # 获取当前颜色值 (b,g,r) = img[row,col] # 将10×10区域内的颜色值改成一致 for i in range(10): for j in range(10): img[row+i,col+j]= (b,g,r) # 显示效果图 cv.imshow('dstimg',img) cv.waitKey(0) cv.destroyAllWindows()py_opencv_毛玻璃效果
import cv2 as cv import numpy as np import random # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) # 获取原图信息 imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 创建一个和原图同样大小的空白矩阵 dstImg = np.zeros((height,width,3),np.uint8) # 定义偏移量,表示从后面6个像素中随机选取一个颜色值 offset=6 # 向空白矩阵中填入颜色值 for row in range(height): for col in range(width): # 计算随机数 index = int(random.random()*offset) # 计算随机行号 randomRow = row + index if row+index<height else height-1 # 计算随机列号 randomCol = col + index if col+index<width else width-1 # 选择附近的一个像素点颜色 (b,g,r) = img[randomRow,randomCol] # 填充到空白矩阵中 dstImg[row,col]=(b,g,r) # 显示效果图 cv.imshow('dstimg',dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_图片浮雕效果
import cv2 as cv import numpy as np import random # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 将图片转成灰度图片 grayImg = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # 创建一张与原图相同大小的空白矩阵 dstImg = np.zeros((height,width,1),np.uint8) # 向空白图片中填充内容 for row in range(height): for col in range(width-1): # 获取当前像素值 gray0 = grayImg[row,col] # 获取相邻一个像素点灰度 gray1 = grayImg[row,col+1] # 计算新的灰度值 gray = int(gray0)-int(gray1)+120 # 校验gray是否越界 gray = gray if gray<255 else 255 gray = gray if gray>0 else 0 # 将值填充到空白的矩阵中 dstImg[row,col] = gray # 显示图片 cv.imshow("dstimg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_绘制图形_line_rectangle_circle
import cv2 as cv import numpy as np import random # 创建一个空白的矩阵 dstImg = np.zeros((400,600,3),np.uint8) # 绘制线段 参数2：起始点 参数3：结束点 参数4：颜色 参数5：线条宽度 cv.line(dstImg,(50,10),(400,10),(255,255,0),10) # 扛锯齿 cv.line(dstImg,(50,50),(400,50),(255,0,0),10,cv.LINE_AA) # 绘制一个三角形 cv.line(dstImg,(50,350),(150,200),(255,0,0),10) cv.line(dstImg,(150,200),(300,350),(0,255,0),10) cv.line(dstImg,(300,350),(50,350),(0,0,255),10) # 绘制一个矩形 参数2： 左上角 参数3：右下角 参数4：颜色 参数5：线条宽度，若为负数，则填充整个矩形 cv.rectangle(dstImg,(250,90),(470,180),(0,255,0),-1) # 绘制圆形 参数2：圆心 参数3：半径 参数4：颜色 参数5：线条宽度 cv.circle(dstImg,(450,280),90,(0,0,255),5) # 显示图片 cv.imshow("dstimg",dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_文字图片绘制
# 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] # 在图片上绘制矩形 cv.rectangle(img,(380,150),(666,236),(0,255,0),5) # 绘制图片 lenaImg = cv.imread("img/123.jpg",cv.IMREAD_COLOR) # 获取图片信息 imgInfo = lenaImg.shape # 计算缩放图片宽高 scale = 0.3 height = int(imgInfo[0]*scale) width = int(imgInfo[1]*scale) # 缩放图片 newLenaImg = cv.resize(lenaImg,(height,width)) # 遍历 for row in range(height): for col in range(width): img[50+row,100+col]=newLenaImg[row,col] cv.imshow('dstimg',img) cv.waitKey(0) cv.destroyAllWindows()注意: opencv不支持中文显示,若想显示中文,需要额外安装中文字体库
py_opencv_直方图均衡化_equalizeHist 美化加亮
import cv2 as cv import matplotlib.pyplot as plt img = cv.imread("img/itheima.jpg",cv.IMREAD_COLOR) b,g,r = cv.split(img) b_dst = cv.equalizeHist(b) g_dst = cv.equalizeHist(g) r_dst = cv.equalizeHist(r) # 会自己统计直方图，但是需要将多行数据转成单行数据 plt.hist(equalize_img.ravel(),bins=256) plt.show() ret = cv.merge([b_dst,g_dst,r_dst]) # 显示均衡化之后的图片 cv.imshow("src",img) cv.imshow("equalize_img",ret) cv.waitKey(0) cv.destroyAllWindows()py_opencv_视频分解图片_VideoCaptur
import cv2 as cv video = cv.VideoCapture("img/twotiger.avi") # 判断视频是否打开成功 isOpened = video.isOpened() print("视频是否打开成功：",isOpened) # 获取图片的信息:帧率 fps = video.get(cv.CAP_PROP_FPS) # 获取每帧宽度 width = video.get(cv.CAP_PROP_FRAME_WIDTH) # 获取每帧的高度 height = video.get(cv.CAP_PROP_FRAME_HEIGHT) print("帧率：{},宽度：{},高度：{}".format(fps,width,height)) # 从视频中读取8帧信息 count=0 while count<8: count = count + 1 # 读取成功or失败， 当前帧数据 flag,frame = video.read() cv.imshow("frame1", frame);#显示视频 # 将图片信息写入到文件中 if flag: # 保存图片的质量 cv.imwrite("img/tiger%d.jpg"%count,frame,[cv.IMWRITE_JPEG_QUALITY,100]) print("图片截取完成啦！")py_opencv_图片合成视频
import cv2 as cv # 读取一张图片 img1 = cv.imread("img/tiger1.jpg",cv.IMREAD_COLOR) # 获取图片信息 imgInfo = img1.shape # 定义宽高 size = (imgInfo[1],imgInfo[0]) # 定义视频写入 编码格式 fourcc = cv.VideoWriter_fourcc(*'XVID') # 输出视频名称 编码格式对象 帧率 大小 videoWrite = cv.VideoWriter("img/tiger_copy2.avi",fourcc,4,size) for i in range(1,8): img = cv.imread("img/tiger%d.jpg"%i) videoWrite.write(img) # 养成良好习惯，释放资源 videoWrite.release() cv.destroyAllWindows() print("生成视频结束！")py_opencv_人脸识别_detectMultiScale
import cv2 as cv # 第1步：加载xml文件 faces_xml = cv.CascadeClassifier("assets/haarcascade_frontalface_default.xml") eyes_xml = cv.CascadeClassifier("assets/haarcascade_eye.xml") # 第2步：加载图片 img = cv.imread("img/lena.jpg", cv.IMREAD_COLOR) # 第3步：将图片转成灰色图片 gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # 第4步：使用api进行人脸识别 参数2：缩放系数 参数3：至少要检测几次才算正确 faces = faces_xml.detectMultiScale(gray, 1.3, 5) print("找到人脸的数量：",len(faces)) # 在人脸上绘制矩形 for (x,y,w,h) in faces: # 在找到人脸上画矩形 cv.rectangle(img,(x,y),(x+w,y+h),(0,255,0),5) # 从灰色图片中找到人脸 grayFace = gray[y:y+h,x:x+w] colorFace = img[y:y+h,x:x+w] # 在当前人脸上找到眼睛的位置 eyes = eyes_xml.detectMultiScale(grayFace,1.3,5) print("当前人脸上眼睛数量：",len(eyes)) # 在眼睛上绘制矩形 for (e_x,e_y,e_w,e_h) in eyes: cv.rectangle(colorFace,(e_x,e_y),(e_x+e_w,e_y+e_h),(0,0,255),3) cv.imshow('result',img) cv.waitKey(0) cv.destroyAllWindows()py_opencv_HSV颜色模型_cvtColor
import cv2 as cv import numpy as np def average_brightness(img): """封装一个计算图片平均亮度的函数""" imgInfo = img.shape height = imgInfo[0] width = imgInfo[1] hsv_img = cv.cvtColor(img, cv.COLOR_BGR2HSV) # 提取出v通道信息 v_day = cv.split(hsv_img)[2] # 计算亮度之和 result = np.sum(v_day) # 返回亮度的平均值 return result/(height*width) # 计算白天的亮度平均值 day_img = cv.imread("assets/day.jpg", cv.IMREAD_COLOR) brightness1 = average_brightness(day_img) print("day brightness1:",brightness1); # 计算晚上的亮度平均值 night_img = cv.imread("assets/night.jpg", cv.IMREAD_COLOR) brightness2 = average_brightness(night_img) print("night brightness2:",brightness2) cv.waitKey(0) cv.destroyAllWindows()py_opencv_抠图合成_inRange
import cv2 as cv # 1.读取绿幕图片 green_img = cv.imread("img/lion.jpg", cv.IMREAD_COLOR) hsv_img = cv.cvtColor(green_img,cv.COLOR_BGR2HSV) # 2. 定义绿幕的颜色范围 lower_green = (35,43,60) upper_green = (77,255,255) # 3. 使用inrange找出所有的背景区域 mask_green = cv.inRange(hsv_img, lower_green, upper_green) # 复制狮子绿幕图片 mask_img = green_img.copy() # 将绿幕图片，对应蒙板图片中所有不为0的地方全部改成0 mask_img[mask_green!=0]=(0,0,0) cv.imshow("dst",mask_img) # itheima图片 对应蒙板图片为0的地方全都改成0,抠出狮子要存放的位置 itheima_img = cv.imread("img/itheima.jpg", cv.IMREAD_COLOR) itheima_img[mask_green==0]=(0,0,0) cv.imshow("itheima",itheima_img) # 将抠出来的狮子与处理过的itheima图片加载一起 result = itheima_img+mask_img cv.imshow("result",result) cv.waitKey(0) cv.destroyAllWindows()py_opencv_简单二值图_threshold 所使用的阈值,结果图片 = cv.threshold(img,阈值,最大值,类型)
import cv2 as cv """ 简单阈值 彩图bgr（255,255,255） ---> 灰度图gray（255）---》二值图 0,1 图像处理基本步骤： 1.彩图 2.灰度图 3.二值图 所使用的阈值,结果图片 = cv.threshold(img,阈值,最大值,类型) """ src = cv.imread("../img/car.jpg") cv.imshow("src",src); # 转成灰度图 gray = cv.cvtColor(src,cv.COLOR_BGR2GRAY); def onChange(val): # 直接调用api处理 返回值1：使用的阈值， 返回值2：处理之后的图像 thresh, binary = cv.threshold(gray, val, 255, cv.THRESH_BINARY_INV); cv.imshow("binary", binary) onChange(0); # thresh = 120; # maxval = 255; # 直接调用api处理 返回值1：使用的阈值， 返回值2：处理之后的图像 # thresh,binary = cv.threshold(gray,thresh,maxval,cv.THRESH_BINARY_INV); # cv.imshow("binary",binary) # 创建滑动条 cv.createTrackbar("thresh","binary",0,255,onChange); cv.waitKey(); cv.destroyAllWindows()py_opencv_自适应二值图_adaptiveThreshold
import cv2 as cv #自适应阈值 # 读取图像 img = cv.imread("assets/thresh1.jpg",cv.IMREAD_GRAYSCALE) # 显示图片 cv.imshow("gray",img) # 获取图片信息 imgInfo = img.shape # 直接调用api处理 参数1：图像数据 参数2：最大值 参数3：计算阈值的方法， 参数4：阈值类型 参数5：处理块大小 参数6：算法需要的常量C thresh_img = cv.adaptiveThreshold(img,255,cv.ADAPTIVE_THRESH_GAUSSIAN_C,cv.THRESH_BINARY,11,5) # 显示修改之后的图片 cv.imshow("thresh",thresh_img); cv.waitKey(0) cv.destroyAllWindows()py_opencv_自适应二值图大类间方差法_THRESH_OTSU
import cv2 as cv # 读取图像 img = cv.imread("assets/otsu_test.png",cv.IMREAD_GRAYSCALE) cv.imshow("src",img) #所使用的阈值,结果图片 = cv.threshold(img,阈值,最大值,类型) # 直接调用api处理 返回值1：使用的阈值， 返回值2：处理之后的图像 ret,thresh_img = cv.threshold(img, 225, 255, cv.THRESH_BINARY_INV) cv.imshow("normal", thresh_img); gaussian_img = cv.GaussianBlur(img,(5,5),0) cv.imshow("g",gaussian_img) # 直接调用api处理 返回值1：使用的阈值， 返回值2：处理之后的图像 ret,thresh_img = cv.threshold(gaussian_img, 0, 255, cv.THRESH_BINARY|cv.THRESH_OTSU) cv.imshow("otsu", thresh_img); print("阈值：",ret) cv.waitKey(0) cv.destroyAllWindows()py_opencv_均值滤波_blur
import cv2 as cv img = cv.imread("./assets/itheima.jpg", cv.IMREAD_COLOR) cv.imshow("src",img) # 参数2: 滤波器的大小应该是奇数，这样它才有一个中心，例如3x3，5x5或者7x7 dst = cv.blur(img, (3,3)) cv.imshow("dst",dst) cv.waitKey(0) cv.destroyAllWindows()py_opencv_图片卷积_filter2D
import cv2 as cv import numpy as np src = cv.imread("../img/itheima_salt.jpg"); # 自定义卷积核 kernel = np.array([[1,0,0], [-2,9,0], [1,-2,1]]) print(kernel) dst = cv.filter2D(src,-1,kernel); cv.imshow("dst",dst); cv.imshow("src",src); cv.waitKey(); cv.destroyAllWindows()py_opencv_高斯模糊_GaussianBlur
import cv2 as cv # 回调函数 def updateSigma(val): # 高斯模糊 参数1:图像 参数2:卷积核大小, 参数3:标准差越大，去除高斯噪声能力越强，图像越模糊 gaussian_blur = cv.GaussianBlur(img, (5,5), val) cv.imshow("gaussian",gaussian_blur) img = cv.imread("assets/itheima.jpg", cv.IMREAD_GRAYSCALE) cv.imshow("src",img) # 创建一个窗口 cv.namedWindow("gaussian",cv.WINDOW_AUTOSIZE) # 创建一个窗口进度条: 参数1:名称 参数2:窗口名称 参数3: 起始值 参数4: 最大值, 参数5:回调函数 cv.createTrackbar("sigma","gaussian",0,255,updateSigma) updateSigma(0) cv.waitKey(0) cv.destroyAllWindows()py_opencv_中值滤波_medianBlur
import cv2 as cv img = cv.imread("./assets/itheima_salt.jpg", cv.IMREAD_COLOR) cv.imshow("src",img) # 参数2:3*3的像素 dst = cv.medianBlur(img, 3) cv.imshow("dst",dst) cv.waitKey(0) cv.destroyAllWindows()py_opencv_Sobel算子边缘检测
import cv2 as cv img = cv.imread("./assets/brain.jpg",cv.IMREAD_GRAYSCALE) cv.imshow("src",img) # sobel算子 参数1:图像, 参数2:图像的深度 -1表示和原图相同, 参数3: x方向求导的阶数 参数4: y方向求导的阶数 x_sobel = cv.Sobel(img, cv.CV_32F, 1, 0) # 将图像转成8位int x_sobel = cv.convertScaleAbs(x_sobel) cv.imshow("x sobel",x_sobel) # sobel算子 #y_sobel = cv.Sobel(img, cv.CV_16S, 0, 1) # 将图像转成8位int #y_sobel = cv.convertScaleAbs(y_sobel) #cv.imshow("y_sobel",y_sobel) y_sobel = cv.Sobel(img,-1,0,1);#参数2 -1表示和原图相同,就不需要转8位int了 cv.imshow("y_sobel",y_sobel); # 将x，y方向的内容叠加起来 x_y_sobel = cv.addWeighted(x_sobel, 0.5, y_sobel, 0.5,0) cv.imshow("x,y sobel",x_y_sobel) cv.waitKey(0) cv.destroyAllWindows()y_sobel = cv.Sobel(img,-1,0,1);#参数2 -1表示和原图相同,就不需要转8位int了 cv.imshow("y_sobel",y_sobel);
py_opencv_Scharr算子边缘检测
import cv2 as cv img = cv.imread("./assets/brain.jpg",cv.IMREAD_GRAYSCALE) cv.imshow("src",img) # sobel算子 x_scharr = cv.Scharr(img, cv.CV_32F, 1, 0) # 将图像转成8位int x_scharr = cv.convertScaleAbs(x_scharr) cv.imshow("x scharr",x_scharr) # # sobel算子 #y_scharr = cv.Scharr(img, cv.CV_16S, 0, 1) # 将图像转成8位int #y_scharr = cv.convertScaleAbs(y_scharr) #cv.imshow("y scharr",y_scharr) y_sobel = cv.Scharr(img,-1,0,1);#参数2 -1表示和原图相同,就不需要转8位int了 cv.imshow("y_sobel",y_sobel); # 将x，y方向的内容叠加起来 xy_scharr = cv.addWeighted(x_scharr, 0.5, y_scharr, 0.5,0) cv.imshow("x,y scharr",xy_scharr) cv.waitKey(0) cv.destroyAllWindows()py_opencv_Laplacian拉普拉斯算子边缘检测
import cv2 as cv img = cv.imread("./assets/grbq.jpg",cv.IMREAD_GRAYSCALE) cv.imshow("src",img) # 使用拉普拉斯算子 #dst = cv.Laplacian(img,cv.CV_32F) # 取绝对值，将数据转到uint8类型 #dst = cv.convertScaleAbs(dst) # 使用拉普拉斯算子 dst = cv.Laplacian(img,-1)#参数2 -1表示和原图相同,就不需要转8位int了 cv.imshow("dst",dst) cv.waitKey(0) cv.destroyAllWindows();py_opencv_canny边缘检测 常用
import cv2 as cv import numpy as np import random # 将图片数据读取进来 img = cv.imread("img/123.jpg",cv.IMREAD_COLOR) cv.imshow("img",img) # 1. 将图片转成灰度图片 grayImg = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # 2. canny算法 参数2: T2:T1 = 3:1 / 2:1 T2为高阈值，T1为低阈值 dstImg = cv.Canny(grayImg,50,180) # 显示效果图 cv.imshow('dstimg',dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_bilateralFilter双边滤波磨皮
cv.bilateralFilter(输入图像, d, sigmaColor, sigmaSpace)
src: 输入图像 d: 表示在过滤过程中每个像素邻域的直径范围。如果这个值是非正数，则函数会从sigmaSpace计算该值。 sigmaColor: 颜色空间过滤器的sigma值，这个参数的值越大，表明该像素邻域内有越宽广的颜色会被混合到一起，产生较大的半相等颜色区域。 sigmaSpace: 坐标空间中滤波器的sigma值，如果该值较大，则意味着越远的像素将相互影响，从而使更大的区域中足够相似的颜色获取相同的颜色.
import cv2 as cv ''' cv.bilateralFilter(输入图像, d, sigmaColor, sigmaSpace) src: 输入图像 d: 表示在过滤过程中每个像素邻域的直径范围。如果这个值是非正数，则函数会从sigmaSpace计算该值。 sigmaColor: 颜色空间过滤器的sigma值，这个参数的值越大，表明该像素邻域内有越宽广的颜色会被混合到一起，产生较大的半相等颜色区域。 sigmaSpace: 坐标空间中滤波器的sigma值，如果该值较大，则意味着越远的像素将相互影响，从而使更大的区域中足够相似的颜色获取相同的颜色. ''' # 将图片数据读取进来 img = cv.imread("img/timg.jpg",cv.IMREAD_COLOR) cv.imshow('img',img) # 双边滤波 dstImg = cv.bilateralFilter(img, 10, 50, 50) # 显示改变之后的图像 cv.imshow('newimg',dstImg) cv.waitKey(0) cv.destroyAllWindows()py_opencv_filter2D锐化滤波
import cv2 as cv import numpy as np img = cv.imread("./assets/hehua.jpg",cv.IMREAD_COLOR) cv.imshow("src",img) kernel = np.array([ [-1,-1,-1], [-1,9,-1], [-1,-1,-1]]) dst = cv.filter2D(img,-1,kernel) cv.imshow("sharpness filter",dst) cv.waitKey(0) cv.destroyAllWindows()py_opencv_HoughLinesP霍夫直线
import cv2 as cv import matplotlib.pyplot as plt import numpy as np # 1. 将图片以灰度的方式读取进来 img = cv.imread("assets/weiqi.jpg", cv.IMREAD_COLOR) cv.imshow("src",img) gray_img = cv.cvtColor(img,cv.COLOR_BGR2GRAY) # cv.imshow("gray",gray_img) # flag,thresh_img = cv.threshold(gray_img,100,255,cv.THRESH_BINARY_INV) cv.imshow("thresh_img",thresh_img) # 3. 霍夫变换 # 线段以像素为单位的距离精度，double类型的，推荐用1.0 rho = 1 # 线段以弧度为单位的角度精度，推荐用numpy.pi/180 theta = np.pi/180 # 累加平面的阈值参数，int类型，超过设定阈值才被检测出线段，值越大，基本上意味着检出的线段越长，检出的线段个数越少。 threshold=10 # 线段以像素为单位的最小长度 min_line_length=25 # 同一方向上两条线段判定为一条线段的最大允许间隔（断裂），超过了设定值，则把两条线段当成一条线段，值越大，允许线段上的断裂越大，越有可能检出潜在的直线段 max_line_gap = 3 lines = cv.HoughLinesP(thresh_img,rho,theta,threshold,minLineLength=min_line_length,maxLineGap=max_line_gap) dst_img = img.copy() for line in lines: x1,y1,x2,y2 = line[0] cv.line(dst_img,(x1,y1),(x2,y2),(0,0,255),2) cv.imshow("dst img",dst_img) cv.waitKey(0) cv.destroyAllWindows()py_opencv_HoughCircles霍夫直线
import cv2 as cv import numpy as np img = cv.imread("assets/weiqi.jpg", cv.IMREAD_COLOR) cv.imshow("src",img) # 将图片转成灰色图片 gray_img = cv.cvtColor(img,cv.COLOR_BGR2GRAY) # 霍夫圆形检测 def hough_circle(gray_img): # 定义检测图像中圆的方法。目前唯一实现的方法是cv2.HOUGH_GRADIENT method = cv.HOUGH_GRADIENT # 累加器分辨率与图像分辨率的反比。例如，如果dp = 1，则累加器具有与输入图像相同的分辨率。如果dp = 2，则累加器的宽度和高度都是一半。 dp = 1 # 检测到的圆的圆心之间最小距离。如果minDist太小，则可能导致检测到多个相邻的圆。如果minDist太大，则可能导致很多圆检测不到。 minDist = 20 # param1 Canny算法阈值上线 # param2 cv2.HOUGH_GRADIENT方法的累加器阈值。阈值越小，检测到的圈子越多。 # minRadius : 最小的半径，如果不确定，则不指定 # maxRadius : 最大的半径，若不确定，则不指定 circles = cv.HoughCircles(gray_img,method,dp,minDist=minDist,param1=70,param2=30,minRadius=0,maxRadius=20) for circle in circles[0,:]: # 圆心坐标，半径 x,y,r = circle # 绘制圆心 cv.circle(img,(x,y),2,(0,255,0),1) # 绘制圆形 cv.circle(img,(x,y),r,(0,0,255),2) cv.imshow("result",img) # 调用函数，寻找霍夫圆 hough_circle(gray_img) cv.waitKey(0) cv.destroyAllWindows()py_opencv_使用matplotlib绘制直方图有两种方式 -----------------------------
方式一: # 使用api将直方图数据计算好 图片 通道 掩膜 数量 值的范围 hist = cv.calcHist([grayImg],[0],None,[256],[0.0,255.0]) # 调用plot函数显示 plt.plot(hist,color="green") plt.show() 方式二: # 1.使用Img.ravel()将多行的矩阵转成单行的矩阵 # 2. 然后调用matplot的hist函数自动计算直方图,bins表示像素区间数量 plt.hist(grayImg.ravel(),color="red",bins=256) plt.show()py_opencv_findContours边缘与轮廓_及轮廓信息
import cv2 as cv # 彩图 ---》 灰度图 ---》 二值图 src = cv.imread("../img/shape.jpg"); gray = cv.cvtColor(src,cv.COLOR_BGR2GRAY); # 将灰度图转成二值图 thresh , binary = cv.threshold(gray,0,255,cv.THRESH_BINARY_INV|cv.THRESH_OTSU); # 查找轮廓 4.x 3.4 _,contours,hierarchy = cv.findContours(binary,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE); print("找到的轮廓数量：",len(contours)); #hierarchy[i][3]，分别表示第i个轮廓的后一个轮廓、前一个轮廓、父轮廓、内嵌轮廓的索引编号 print("轮廓的层级关系：",hierarchy); print("第0个轮廓点的数量：",len(contours[0])) # 轮廓的索引 -1 绘制所有轮廓 cv.drawContours(src,contours,0,(255,255,0),2); # 遍历每一个轮廓 for contour in contours: # 计算轮廓的面积 area = cv.contourArea(contour); print("面积：",area); # 外切圆 center,radius = cv.minEnclosingCircle(contour); x = int(center[0]) y = int(center[1]) radius = int(radius) cv.circle(src,(x,y),radius,(255,0,0),2); # 包围矩形 rect = cv.boundingRect(contour); x,y,w,h = rect; cv.rectangle(src,(x,y),(x+w,y+h),(0,255,0),2); # 最小包围矩形 ratationRect = cv.minAreaRect(contour); # print(ratationRect); length = cv.arcLength(contour,True); print("周长：",length); cv.imshow("src",src); cv.imshow("binary",binary); # cv.imshow("gray",gray); cv.waitKey(); cv.destroyAllWindows()py_opencv_dilate膨胀
import cv2 as cv # src = cv.imread("../img/morph-j.jpg") src = cv.imread("../img/lena.jpg") kernel = cv.getStructuringElement(cv.MORPH_RECT,(10,10)); print(kernel) # 膨胀 dst = cv.dilate(src,kernel); cv.imshow("dst",dst); cv.imshow("src",src); cv.waitKey(); cv.destroyAllWindows()py_opencv_erode腐蚀
import cv2 as cv src = cv.imread("../img/morph-j.jpg") src = cv.imread("../img/lena.jpg") kernel = cv.getStructuringElement(cv.MORPH_RECT,(5,5)); dst = cv.erode(src,kernel); cv.imshow("dst",dst); cv.imshow("src",src); cv.waitKey(); cv.destroyAllWindows()py_opencv_morphologyEx_开操作Opening
import cv2 as cv src = cv.imread("../img/morph-opening.jpg") # blur_img = cv.medianBlur(src,5) # cv.imshow("blur img",blur_img) kernel = cv.getStructuringElement(cv.MORPH_RECT,(3,3)) # 开操作 : 先腐蚀，后膨胀 dst = cv.morphologyEx(src,cv.MORPH_OPEN,kernel) cv.imshow("dst",dst) # # 腐蚀 # erode_img = cv.erode(src,kernel) # cv.imshow("erode_img",erode_img) # # # 膨胀 # dilate_img = cv.dilate(erode_img,kernel) # cv.imshow("dilate_img",dilate_img) cv.imshow("src",src) cv.waitKey() cv.destroyAllWindows()py_opencv_morphologyEx_闭操作Closing
import cv2 as cv src = cv.imread("../img/morph-closing.jpg") kernel = cv.getStructuringElement(cv.MORPH_RECT,(3,3)) # 先对图像进行膨胀 # dilate_img = cv.dilate(src,kernel) # cv.imshow("dilate",dilate_img) # # # 腐蚀 # erode_img = cv.erode(dilate_img,kernel) # cv.imshow("erode_img",erode_img) dst = cv.morphologyEx(src,cv.MORPH_CLOSE,kernel) cv.imshow("dst",dst) cv.imshow("src",src) cv.waitKey() cv.destroyAllWindows()py_opencv_matchTemplate模板匹配
import cv2 as cv # 原图 src = cv.imread("../img/zhaonimei.jpg") # 模板图 temp = cv.imread("../img/mei.jpg") h,w = temp.shape[0:2] # 模板匹配 res = cv.matchTemplate(src,temp,cv.TM_SQDIFF_NORMED) print(res) # 找出最小值出现的位置 minVal, maxVal, minLoc, maxLoc = cv.minMaxLoc(res) print("最小值：{}，最大值：{}，minLoc={},maxloc={}".format(minVal,maxVal,minLoc,maxLoc)) # 绘制找到的位置 cv.circle(src,minLoc,2,(0,0,255),2) cv.rectangle(src,minLoc,(minLoc[0]+w,minLoc[1]+h),(255,255,0),2) cv.imshow("src",src) cv.imshow("temp",temp) cv.waitKey() cv.destroyAllWindows()py_opencv_normalize将结果进行归一化处理
# 将结果进行归一化处理 cv.normalize(result,result,0,1,cv.NORM_MINMAX)py_opencv_createBackgroundSubtractorMOG2背景消除
import numpy as np import numpy.random as rnd; import cv2 cap = cv2.VideoCapture('../img/vtest.avi') fgbg = cv2.createBackgroundSubtractorMOG2(detectShadows=True); while(1): ret, frame = cap.read() fgmask = fgbg.apply(frame) blur_img = cv2.medianBlur(fgmask,5); cv2.imshow('frame',blur_img) _,contours,_ = cv2.findContours(blur_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE); for contour in contours: area = cv2.contourArea(contour); if area >1000: x,y,w,h = cv2.boundingRect(contour); cv2.rectangle(frame,(x,y),(x+w,y+h),(rnd.randint(0,255),rnd.randint(0,255),rnd.randint(0,255)),2) cv2.imshow("src",frame); k = cv2.waitKey(30) & 0xff if k == 27: break cap.release() cv2.destroyAllWindows()py_opencv_fillPoly绘制多边形
# 绘制多边形 三角形的三个点 polygon = np.array([[[260,height],[1100,height],[570,283]]]) # 创建一个空白的画布 mask_img = np.zeros_like(img); # 填充空白的画布： 三角形 . 绘制一个填充的三角形 cv.fillPoly(mask_img,polygon,255); cv.imshow("mask",mask_img);py_opencv_convertScaleAbs数据类型转换
# 使用拉普拉斯算子 #dst = cv.Laplacian(img,cv.CV_32F) # 取绝对值，将数据转到uint8类型 #dst = cv.convertScaleAbs(dst)py_opencv_glob获取路径下所有文件
import numpy as np import cv2 import glob for fname in glob.glob('image_*.jpg'): img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (6, 9), None)py_opencv_fillPoly_bitwise_and填充ROI掩膜_将感兴趣区域填充成白色
h, w = img.shape[:2] lower = 150 upper = 255 dst = cv2.Canny(img2, lower, upper) roi = np.array([ [200, 20], [600, 20], [600, 320], [200, 320] ], dtype=np.int32) # 创建纯黑的原图掩膜 roi_mask = np.zeros([h, w], np.uint8) # 填充ROI掩膜，将感兴趣区域填充成白色 cv.fillPoly(roi_mask, [roi], 255) # 通过像素位与操作保留刚刚掩膜中白色的区域 img_roi = cv.bitwise_and(dst, roi_mask)py_opencv_inRange_createTrackbar_HSV多参数调参
i1 = 0 i2 = 0 i3 = 0 i4 = 0 i5 = 0 i6 = 0 def onChange(a,b): print(a) print(b) global i1,i2,i3,i4,i5,i6 if a==1: i1 = b elif a==2: i2 = b elif a == 3: i3 = b elif a == 4: i4 = b elif a == 5: i5 = b elif a == 6: i6 = b lowerb = (i1, i2, i3) upperb = (i4, i5, i6) mask = cv.inRange(hsv, lowerb, upperb) cv.imshow("mask", mask) print(i1,i2,i3,i4,i5,i6) if __name__ == '__main__': img = cv.imread("data/pic9.png") cv.imshow("img", img) hsv=cv.cvtColor(img,cv.COLOR_BGR2HSV) cv.namedWindow("mask") cv.createTrackbar("h1", "mask", 0, 255, lambda a : onChange(1,a)) cv.createTrackbar("s1", "mask", 0, 255, lambda a: onChange(2, a)) cv.createTrackbar("v1", "mask", 0, 255, lambda a: onChange(3, a)) cv.createTrackbar("h2", "mask", 0, 255, lambda a: onChange(4, a)) cv.createTrackbar("s2", "mask", 0, 255, lambda a: onChange(5, a)) cv.createTrackbar("v2", "mask", 0, 255, lambda a: onChange(6, a)) cv.waitKey(0) cv.destroyAllWindows()
py_opencv_findContours_minAreaRect_getRotationMatrix2D查找轮廓按最小矩形旋转 ====
_, contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) print(len(contours)) for contour in contours: rect=cv2.minAreaRect(contour) x1=int(rect[0][0]) y1=int(rect[0][1]) w1=int(rect[1][0]) h1=int(rect[1][1]) angle=rect[2] robot =cv2.getRotationMatrix2D((x1,y1),angle,1) img_rotated_by_alpha = cv2.warpAffine(img, robot, (img.shape[1], img.shape[0])) print("img_rotated_by_alpha{}".format(img_rotated_by_alpha)) cv2.imshow("robot", img_rotated_by_alpha) cv2.destroyAllWindows()py_opencv_findContours查找轮廓绘画矩形_最小矩形_外切园
_, contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) print("找到轮廓个数:", len(contours)) temp_img = img.copy() cv2.drawContours(temp_img, contours, -1, (0, 255, 255), 3) for i, cnt in enumerate(contours): # cv2.drawContours(img, contours, i, (0, 255, 255), 2) # 获取矩形区域 x, y, w, h = cv2.boundingRect(cnt) # 计算多边形的矩 mm = cv2.moments(cnt) if mm['m00'] == 0: continue cx = mm['m10'] / mm['m00'] cy = mm['m01'] / mm['m00'] # 计算周长 cnt_len = cv2.arcLength(cnt, True) # 获取近似曲线 # 参数2：epsilon 逼近程度阈值 # 参数1：close 是否为闭合区域 approxCurve = cv2.approxPolyDP(cnt, cnt_len * 0.02, True) curve = approxCurve.shape[0] # 计算轮廓的面积 area = cv2.contourArea(cnt) print(cv2.isContourConvex(cnt)) # 面积大于1000, 边个数至少4个 if area > 1000 and curve >= 4: # 绘制正矩形框 cv2.rectangle(final_img,(x,y),(x+w,y+h),(0,255,0),3) # 绘制中心 cv2.circle(final_img, (np.int(cx), np.int(cy)), 5, (0, 0, 255), -1) # 计算最小矩形区域 rect = cv2.minAreaRect(cnt) # 获取盒子顶点 # 绘制最小矩形 1.对应左下角，2.对应左上角，3.对应右上角，4对应右下角 # box: [[464 311] 1.对应左下角 # [246 237] 2.对应左上角 # [263 186] 3.对应右上角 # [481 259]]4对应右下角 box = cv2.boxPoints(rect) # 转成long类型 box = np.int0(box) # 绘制最小矩形 cv2.drawContours(final_img, [box], 0, (255, 0, 0), 2) # cv2.drawContours(final_img, contours, i, (255, 0, 0), 3) print("{}------宽高比: {} 中心: {} 面积: {} 边个数：{}".format(i, min(w, h) / max(w, h), \ (round(cx,2),(round(cy,2))), area, curve))py_opencv_findContours查找轮廓最小矩形外切矩形
#binary 二值图 _, contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for contour in contours: # 计算最小矩形区域 rect=cv2.minAreaRect(contour) x1 = int(rect[0][0]) #中心点 y1 = int(rect[0][1]) #中心点 w1 = int(rect[1][0]) #宽 h1 = int(rect[1][1]) #高 angle = rect[2] # 角度 # 获取盒子顶点 box = cv2.boxPoints(rect) # 转成long类型 box = np.int0(box) # 绘制最小矩形 1.对应左下角，2.对应左上角，3.对应右上角，4对应右下角 # box: [[464 311] 1.对应左下角 # [246 237] 2.对应左上角 # [263 186] 3.对应右上角 # [481 259]]4对应右下角 cv2.drawContours(img, [box], 0, (255, 0, 255), 2) x3=int(box[0][0]) #左下角 宽 cols w y3=int(box[0][1]) #左下角 高 rows h x2=int(box[1][0]) #左上角 宽 cols w y2=int(box[1][1]) #左上角 高 rows h x1=int(box[2][0]) #右上角 宽 cols w y1=int(box[2][1]) #右上角 高 rows h x4=int(box[3][0]) #右下角 宽 cols w y4=int(box[3][1]) #右下角 高 rows h # 边框 cv2.drawContours(image_copy, [box], 0, (255, 0, 255), 2) #cv2.line(image_copy,(x2-x1,y2-y1),(x3-x4,y3-y4),(255,255,0),1) print("x2-x1:{}".format(x2-x1)) y = int((y3 - y2) / 2) #取高度的中间点 2分之一的线段长度 x = int((x4 - x3) / 2) #取高度的中间点 2分之一的线段长度 #cv2.line(image_copy, (y2, x2), (y1, x1), (255,0 , 0), 3) # 分成4格 cv2.line(image_copy, (x1 , y1+y), (x2 ,y1+y), (255,255, 0), 3) # 中心横线 cv2.line(image_copy, (x2+x, y2 ), (x3+x, y3 ), (255,255, 0), 3) #左中心竖线 # 边框 cv2.line(image_copy, (x1 , y1 ), (x2 , y1), (255, 0, 0), 3) # 上边框 cv2.line(image_copy, (x2 , y2), (x3 , y3), (255, 0, 0), 3) # 左边框 cv2.line(image_copy, (x3 , y3), (x4 , y4), (255, 0, 0), 3) # 下边框 cv2.line(image_copy, (x4 , y4), (x1 , y1), (255, 0, 0), 3) # 右边框 #中心点 4分之一的线段长度 x_4= int((x4 - x3) / 4) # 取宽度的中间点 4分之一的线段长度 y_4 = int((y3 - y2) / 4) # 取高度的中间点 4分之一的线段长度 # 1.中心点 坐标 x_z=x2 + x # 宽 cols w y_z=y1 + y # 高 rows h cv2.circle(image_copy, (x_z, y_z), 5, (255, 0, 255), -1)py_opencv_findContours查找轮廓最小矩形_绘制中心点_及各边框_分格
_,contours, hierarchy = cv2.findContours(image_OPEN,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) print(len(contours)) maxarea=0 maxarea_index=0 for index,contour in enumerate(contours): area = cv2.contourArea(contour) print(area) if area >maxarea: maxarea=area maxarea_index=index print("index{}".format(index)) # 最小矩形 rect = cv2.minAreaRect(contours[maxarea_index]) x1 = int(rect[0][0]) #中心点 y1 = int(rect[0][1]) #中心点 w1 = int(rect[1][0]) #宽 h1 = int(rect[1][1]) #高 angle = rect[2] # 角度 #cv2.drawContours(image_copy,contours,maxarea_index,(255,0,0),1) box = cv2.boxPoints(rect) box= np.int0(box) print("box：{}".format(box)) x3=int(box[0][0]) #左下角 宽 cols w y3=int(box[0][1]) #左下角 高 rows h x2=int(box[1][0]) #左上角 宽 cols w y2=int(box[1][1]) #左上角 高 rows h x1=int(box[2][0]) #右上角 宽 cols w y1=int(box[2][1]) #右上角 高 rows h x4=int(box[3][0]) #右下角 宽 cols w y4=int(box[3][1]) #右下角 高 rows h # 边框 cv2.drawContours(image_copy, [box], 0, (255, 0, 255), 2) #cv2.line(image_copy,(x2-x1,y2-y1),(x3-x4,y3-y4),(255,255,0),1) print("x2-x1:{}".format(x2-x1)) y = int((y3 - y2) / 2) #取高度的中间点 2分之一的线段长度 x = int((x4 - x3) / 2) #取高度的中间点 2分之一的线段长度 #cv2.line(image_copy, (y2, x2), (y1, x1), (255,0 , 0), 3) # 分成4格 cv2.line(image_copy, (x1 , y1+y), (x2 ,y1+y), (255,255, 0), 3) # 中心横线 cv2.line(image_copy, (x2+x, y2 ), (x3+x, y3 ), (255,255, 0), 3) #左中心竖线 # 边框 cv2.line(image_copy, (x1 , y1 ), (x2 , y1), (255, 0, 0), 3) # 上边框 cv2.line(image_copy, (x2 , y2), (x3 , y3), (255, 0, 0), 3) # 左边框 cv2.line(image_copy, (x3 , y3), (x4 , y4), (255, 0, 0), 3) # 下边框 cv2.line(image_copy, (x4 , y4), (x1 , y1), (255, 0, 0), 3) # 右边框 #中心点 4分之一的线段长度 x_4= int((x4 - x3) / 4) # 取宽度的中间点 4分之一的线段长度 y_4 = int((y3 - y2) / 4) # 取高度的中间点 4分之一的线段长度 # 1.中心点 坐标 x_z=x2 + x # 宽 cols w y_z=y1 + y # 高 rows h cv2.circle(image_copy, (x_z, y_z), 5, (255, 0, 255), -1) string_lup = "x {} y {}".format(x_z, y_z) cv2.putText(image_copy, string_lup, (x_z - 50, y_z - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 1, cv2.LINE_AA) # 1.左上 x_lup = x2+x_4 # 宽 cols w y_lup = y1+y_4 # 高 rows h cv2.circle(image_copy,(x_lup,y_lup),5,(255,0,0),-1) string_lup="x {} y {}".format(x_lup,y_lup) cv2.putText(image_copy, string_lup, (x_lup-50, y_lup-20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0, 0), 1, cv2.LINE_AA) # 1.左下 x_ld= x2 + x_4 # 宽 cols w y_ld = y1 + y_4 + y # 高 rows h cv2.circle(image_copy, (x_ld, y_ld), 5, (255, 0, 0), -1) string_lup = "x {} y {}".format(x_ld, y_ld) cv2.putText(image_copy, string_lup, (x_ld - 50, y_ld - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1, cv2.LINE_AA) # 1.右上 x_rup = x2 + x_4 +x # 宽 cols w y_rup = y1 + y_4 # 高 rows h cv2.circle(image_copy, (x_rup, y_rup), 5, (255, 0, 0), -1) string_lup = "x {} y {}".format(x_rup, y_rup) cv2.putText(image_copy, string_lup, (x_rup - 50, y_rup - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1, cv2.LINE_AA) # 1.右下 x_rd= x2 + x_4 + x # 宽 cols w y_rd = y1 + y_4+ y # 高 rows h cv2.circle(image_copy, (x_rd, y_rd), 5, (255, 0, 0), -1) string_lup = "x {} y {}".format(x_rd, y_rd) cv2.putText(image_copy, string_lup, (x_rd - 50, y_rd - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1, cv2.LINE_AA) cv2.imshow("image_copy_drawContours", image_copy) cv2.destroyAllWindows()py_opencv_warpPerspective透视拉伸
# 将目标区域梯形转成 300x300 正方形区域 dst_found = True #原 pts1 = np.float32([rst_optimited[0], rst_optimited[3], rst_optimited[1], rst_optimited[2]]) #300 300 pts2 = np.float32([[0, 0], [rows, 0], [0, rows], [rows, rows]]) M = cv2.getPerspectiveTransform(pts1, pts2) dst_area = cv2.warpPerspective(src, M, (rows, rows))py_opencv_np_zeros创建新的空图片
dstImg = np.zeros((height, width, 1), np.uint8) # 定义图片 左上角，左下角 右上角的坐标 matrixSrc = np.float32([[0, 0], [0, height - 1], [width - 1, 0]]) # 将原来的点映射到新的点 matrixDst = np.float32([[50, 100], [300, height - 200], [width - 300, 100]])py_opencv_保留感兴趣区域ROI
h, w = img2.shape[:2] roi = np.array([ [200, 20], [600, 20], [600, 320],[200, 320] ], dtype=np.int32) # 创建纯黑的原图掩膜 roi_mask = np.zeros([h,w], np.uint8) # 填充ROI掩膜，将感兴趣区域填充成白色 cv2.fillPoly(roi_mask, [roi], 255) # 通过像素位与操作保留刚刚掩膜中白色的区域 img_roi = cv2.bitwise_and(dst, roi_mask) #===================================== # 获取上半段工作台图 dstImg[:int(rows/2),:]=255 image_copy=cv2.bitwise_and(image_copy2,dstImg)py_opencv_bitwise二值文件黑白颠倒
cv2.bitwise_not(image_OPEN, image_OPEN)# 颜色反转 二值文件黑白颠倒c++ 模板:
cpp_ros_main_node节点初始化
#include <iostream> #include <ros/ros.h> using namespace std; int main(int argc,char *argv[]){ //节点名 string nodeName = "$args1$"; //初始化节点 ros::init(argc,argv,nodeName); //创建节点 ros::NodeHandle node; $args2$ //事件轮询 ros::spin(); return 0; }cpp_ros_node节点
//创建节点 ros::NodeHandle node;
1. cpp_qt_class_source_Qt5
#include <QWidget> #include <QVBoxLayout> #include <QHBoxLayout> #include <QPushButton> #include <QLineEdit> #include <QCheckBox> #include <ros/ros.h> #include <std_srvs/Empty.h> #include <turtlesim/Spawn.h> #include <turtlesim/Kill.h> #include <turtlesim/SetPen.h> #include <turtlesim/TeleportAbsolute.h> #include <turtlesim/TeleportRelative.h> #include <iostream> #include "MainWindow.h" MainWindow::MainWindow(ros::NodeHandle node, QWidget *parent) : QWidget(parent), node(node) { } MainWindow::~MainWindow() { ros::shutdown(); }2. cpp_qt_class_head_Qt5
#include <QWidget> #include <QVBoxLayout> #include <QHBoxLayout> #include <QPushButton> #include <QLineEdit> #include <QCheckBox> #include <ros/ros.h> #include <std_srvs/Empty.h> #include <turtlesim/Spawn.h> #include <turtlesim/Kill.h> #include <turtlesim/SetPen.h> #include <turtlesim/TeleportAbsolute.h> #include <turtlesim/TeleportRelative.h> #include <iostream> using namespace std; class MainWindow: public QWidget { public: explicit MainWindow(ros::NodeHandle node,QWidget* parent = Q_NULLPTR); ~MainWindow() override; };3.server
cpp_ros_client_main通讯
#include <iostream> #include <ros/ros.h> #include <roscpp_tutorials/TwoInts.h> using namespace std; int main(int argc,char *argv[]){ //节点名 string nodeName = "cpp_client"; //初始化节点 ros::init(argc,argv,nodeName,ros::init_options::AnonymousName); //创建节点 ros::NodeHandle node; //service名字 string serviceName = "cpp_service"; //创建client ros::ServiceClient client = node.serviceClient<roscpp_tutorials::TwoInts>(serviceName); //等待server端上线 client.waitForExistence(); //定义数据 roscpp_tutorials::TwoInts service; service.request.a = 12; service.request.b = 13; //调用service client.call(service); ROS_INFO_STREAM("result:"<<service.response.sum); //事件轮询 ros::spin(); return 0; }cpp_ros_client通讯
#include <iostream> #include <ros/ros.h> #include <roscpp_tutorials/TwoInts.h> using namespace std; //service名字 string serviceName = "cpp_service"; //创建client ros::ServiceClient client = node.serviceClient<roscpp_tutorials::TwoInts>(serviceName); //等待server端上线 client.waitForExistence(); //定义数据 roscpp_tutorials::TwoInts service; service.request.a = 12; service.request.b = 13; //调用service client.call(service); ROS_INFO_STREAM("result:"<<service.response.sum); //事件轮询 ros::spin();
3.1
cpp_ros_server_main通讯
#include <iostream> #include <ros/ros.h> #include <roscpp_tutorials/TwoInts.h> using namespace std; bool callBack(roscpp_tutorials::TwoIntsRequest& req,roscpp_tutorials::TwoIntsResponse& res){ //从请求中获取两个参数 //把和通过res返回出去 res.sum = req.a+req.b; return true; } int main(int argc,char *argv[]){ //节点名 string nodeName = "cpp_server"; //初始化节点 ros::init(argc,argv,nodeName); //创建节点 ros::NodeHandle node; //service名字 string serviceName = "cpp_service"; //创建server端 const ros::ServiceServer &server = node.advertiseService(serviceName, callBack); //事件轮询 ros::spin(); return 0; }cpp_ros_server通讯
#include <iostream> #include <ros/ros.h> #include <roscpp_tutorials/TwoInts.h> using namespace std; bool callBack(roscpp_tutorials::TwoIntsRequest& req,roscpp_tutorials::TwoIntsResponse& res){ //从请求中获取两个参数 //把和通过res返回出去 res.sum = req.a+req.b; return true; } //service名字 string serviceName = "cpp_service"; //创建server端 const ros::ServiceServer &server = node.advertiseService(serviceName, callBack); //事件轮询 ros::spin();4 topic
cpp_ros_publisher_main通讯
#include <iostream> #include <ros/ros.h> #include <std_msgs/String.h> using namespace std; int main(int argc,char *argv[]){ //节点名 string nodeName = "cpp_publisher"; //初始化节点 匿名节点 ros::init(argc,argv,nodeName,ros::init_options::AnonymousName); //创建节点 ros::NodeHandle node; //节点名 string topicName = "cpp_topic"; //创建topic发送者 //参数1：节点名 //参数2：消息队列容量 const ros::Publisher &publisher = node.advertise<std_msgs::String>(topicName, 5); //每隔一秒钟发送一条消息 ros::Rate rate(1); int i = 0; while (ros::ok()){ std_msgs::String data; data.data = "hello "+to_string(i); //发送一条消息 publisher.publish(data); //增加i i+=1; //睡眠 rate.sleep(); } return 0; }cpp_ros_publishe通讯
#include <iostream> #include <ros/ros.h> #include <std_msgs/String.h> using namespace std; //节点名 string topicName = "cpp_topic"; //创建topic发送者 //参数1：节点名 //参数2：消息队列容量 const ros::Publisher &publisher = node.advertise<std_msgs::String>(topicName, 5); //每隔一秒钟发送一条消息 ros::Rate rate(1); int i = 0; while (ros::ok()){ std_msgs::String data; data.data = "hello "+to_string(i); //发送一条消息 publisher.publish(data); //增加i i+=1; //睡眠 rate.sleep(); }
cpp_ros_subscriber_main通讯
#include <iostream> #include <std_msgs/String.h> #include <ros/ros.h> #include <thread> using namespace std; //c++ 订阅者的回调在主线程中 void callBack(std_msgs::String::ConstPtr data){ cout<<"回调:"<<this_thread::get_id()<<endl; cout<<"收到消息:"<<data->data<<endl; } int main(int argc, char *argv[]) { cout<<"主线程:"<<this_thread::get_id()<<endl; //节点名 string nodeName = "cpp_subscriber"; // string nodeName = "cpp_publisher"; //初始化节点 ros::init(argc, argv, nodeName,ros::init_options::AnonymousName); //创建节点 ros::NodeHandle node; //topic名字 string topicName = "cpp_topic"; //订阅者 //参数1：topic名字 //参数2：队列长度 //参数3：回调函数 //注意：返回的Subscriber必须要接收,如果不接受 可能收不到消息 const ros::Subscriber &subscriber = node.subscribe<std_msgs::String>(topicName, 5, callBack); //ros::spin 处理事件 ros::spin(); return 0; }cpp_ros_subscriber通讯
#include <iostream> #include <std_msgs/String.h> #include <ros/ros.h> #include <thread> using namespace std; //c++ 订阅者的回调在主线程中 void callBack(std_msgs::String::ConstPtr data){ cout<<"回调:"<<this_thread::get_id()<<endl; cout<<"收到消息:"<<data->data<<endl; } //topic名字 string topicName = "cpp_topic"; //订阅者 //参数1：topic名字 //参数2：队列长度 //参数3：回调函数 //注意：返回的Subscriber必须要接收,如果不接受 可能收不到消息 const ros::Subscriber &subscriber = node.subscribe<std_msgs::String>(topicName, 5, callBack); ros::spin();
cpp_QTimer定时器
#include <QApplication> #include <ros/ros.h> #include <QTimer> using namespace std; QTimer timer; MainWindow::MainWindow(){ //定时器 timer.setInterval(10); timer.start(); //定时信号 connect(&timer,&QTimer::timeout,this,&MainWindow::timeout); } //定时的槽函数 判断ros是否关闭,ros关闭后关闭窗口 void MainWindow::timeout(){ //处理消息 if(ros::isShuttingDown()){ QApplication::quit(); } }cpp_getThread_Id 获取线程ID
#include <thread> std::cout<<this_thread::get_id()<<endl;4.action标准通讯
cpp_ros_action_client_main标准通讯
// // Created by wt on 2020/7/6. // #include <iostream> #include <ros/ros.h> #include <actionlib/client/action_client.h> #include <demo_actions/CountNumberAction.h> #include <thread> using namespace std; //状态改变 void transition_cb(actionlib::ClientGoalHandle<demo_actions::CountNumberAction> goalHandle) { // ROS_INFO_STREAM("transition cb"); const actionlib::CommState &commState = goalHandle.getCommState(); //等待 if (commState == actionlib::CommState::PENDING) { ROS_INFO_STREAM("pending"); } else if (commState == actionlib::CommState::ACTIVE) {//激活 ROS_INFO_STREAM("active"); } else if (commState == actionlib::CommState::DONE) {//结束 const actionlib::TerminalState &state = goalHandle.getTerminalState(); if (state == actionlib::TerminalState::REJECTED) { ROS_INFO_STREAM("reject"); } else if (state == actionlib::TerminalState::PREEMPTED) { ROS_INFO_STREAM("preempt"); } else if (state == actionlib::TerminalState::ABORTED) { ROS_INFO_STREAM("aborted "<<goalHandle.getResult()->count); } else if (state == actionlib::TerminalState::SUCCEEDED) { ROS_INFO_STREAM("succeed "<<goalHandle.getResult()->count); } else if (state == actionlib::TerminalState::LOST) {//服务端没有做任何处理 ROS_INFO_STREAM("lost"); } } } //进度回调 void feedback_cb(actionlib::ClientGoalHandle<demo_actions::CountNumberAction> goalHandle, const demo_actions::CountNumberFeedback::ConstPtr &feedback) { ROS_INFO_STREAM("feedback "<<feedback->percent); ROS_INFO_STREAM("feedback "<<this_thread::get_id()); } int main(int argc, char *argv[]) { ROS_INFO_STREAM("main "<<this_thread::get_id()); //节点名 string nodeName = "action_client"; //初始化节点 ros::init(argc, argv, nodeName,ros::init_options::AnonymousName); //创建节点 ros::NodeHandle node; /*-------------------------- asy --------------------------*/ ros::AsyncSpinner spinner(1); spinner.start(); /*-------------------------- action --------------------------*/ //action名字 string actionName = "action_gui"; actionlib::ActionClient<demo_actions::CountNumberAction> client(node, actionName); //等到服务开启(阻塞线程 一直查看服务是否已经开启) client.waitForActionServerToStart(); //目标 demo_actions::CountNumberGoal goal; goal.max = 10; goal.duration = 1; //发送目标 actionlib::ActionClient<demo_actions::CountNumberAction>::GoalHandle handle = client.sendGoal(goal, transition_cb, feedback_cb); /*-------------------------- 取消 --------------------------*/ ros::Rate rate(0.3); rate.sleep(); // handle.cancel(); // client.cancelAllGoals(); //客户端不要停下来 ros::waitForShutdown(); return 0; }
cpp_ros_action_server_main标准通讯
// // Created by wt on 2020/7/6. // #include <iostream> #include <ros/ros.h> #include <actionlib/server/action_server.h> #include <demo_actions/CountNumberAction.h> #include <thread> using namespace std; //被取消 bool isCancel = false; void done_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle); void done_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle) {//2.处理 long max = goalHandle.getGoal()->max; double duration = goalHandle.getGoal()->duration; ros::Rate rate(duration); //结果 demo_actions::CountNumberResult result; for (int i = 1; i < max; ++i) { /*-------------------------- 中断状态 --------------------------*/ // if(i==7){ // //中断 // result.count = i; // goalHandle.setAborted(result); // return; // } /*-------------------------- 取消 --------------------------*/ if(isCancel){ result.count = i; goalHandle.setCanceled(result); return; } //进度 demo_actions::CountNumberFeedback feedback; feedback.percent = (double)i/max; //回调进度 goalHandle.publishFeedback(feedback); //睡眠 rate.sleep(); } /*-------------------------- 成功状态 --------------------------*/ //成功 result.count = max; goalHandle.setSucceeded(result); } //收到目标回调 void goal_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle){ ROS_INFO_STREAM("goal cb "<<this_thread::get_id()); ROS_INFO_STREAM("receive goal"); //拒绝 // goalHandle.setRejected(); //1.决定是否要接受任务 goalHandle.setAccepted(); //开启新线程 new std::thread(done_cb,goalHandle); } //收到取消指令 void cancel_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle){ ROS_INFO_STREAM("cancel goal"); goalHandle.getGoalID().id; isCancel = true; } int main(int argc,char *argv[]){ ROS_INFO_STREAM("thread "<<this_thread::get_id()); //节点名 string nodeName = "action_server"; //初始化节点 ros::init(argc,argv,nodeName); //创建节点 ros::NodeHandle node; /*-------------------------- action --------------------------*/ //action名字 string actionName = "action_gui"; actionlib::ActionServer<demo_actions::CountNumberAction> server(node,actionName,goal_cb,cancel_cb, false); //开启server server.start(); //事件轮询 ros::spin(); return 0; }
cpp_ros_action_client_class标准通讯
#include <QWidget> #include <QLineEdit> #include <QPushButton> #include <QFormLayout> #include <QLabel> #include <actionlib/client/action_client.h> #include <demo_actions/CountNumberAction.h> MainWindowClient::MainWindowClient(ros::NodeHandle node, QWidget *parent) : QWidget(parent), sendBtn("发送"), preemptBtn("取消") { //设置布局 setLayout(&layout); //默认值 maxEdit.setText("10"); durationEdit.setText("1"); //添加控件 layout.addRow("max:", &maxEdit); layout.addRow("duration:", &durationEdit); layout.addRow("进度:", &feedLabel); layout.addRow("激活状态:", &activeLabel); layout.addRow("完成状态:", &doneLabel); layout.addRow("", &sendBtn); layout.addRow("", &preemptBtn); //创建client client = new actionlib::ActionClient<demo_actions::CountNumberAction>(node, "action_gui"); //阻塞当前线程 服务是否连接上 // client->waitForActionServerToStart(); //信号和槽 connect(&sendBtn, &QPushButton::clicked, this, &MainWindowClient::send); connect(&preemptBtn, &QPushButton::clicked, this, &MainWindowClient::cancel); } MainWindowClient::~MainWindowClient() { } void MainWindowClient::send() { //发送目标 demo_actions::CountNumberGoal goal; goal.max = maxEdit.text().toInt(); goal.duration = durationEdit.text().toDouble(); //一定要保存成员变量 handle = client->sendGoal(goal, boost::bind(&MainWindowClient::transition_cb, this, _1), boost::bind(&MainWindowClient::feedback_cb, this, _1, _2)); } void MainWindowClient::transition_cb(actionlib::ClientGoalHandle<demo_actions::CountNumberAction> goalHandle) { const actionlib::CommState &comState = goalHandle.getCommState(); if (comState == actionlib::CommState::ACTIVE) { activeLabel.setText("激活"); } else if (comState == actionlib::CommState::DONE) { const actionlib::TerminalState &terState = goalHandle.getTerminalState(); if (terState == actionlib::TerminalState::REJECTED) { doneLabel.setText("拒绝"); } else if (terState == actionlib::TerminalState::ABORTED) { doneLabel.setText("中止"); } else if (terState == actionlib::TerminalState::PREEMPTED) { doneLabel.setText("取消"); } else if (terState == actionlib::TerminalState::SUCCEEDED) { doneLabel.setText("成功"); } } else if (comState == actionlib::CommState::PENDING) { activeLabel.setText("等待"); } } void MainWindowClient::feedback_cb(actionlib::ClientGoalHandle<demo_actions::CountNumberAction> goalHandle, const demo_actions::CountNumberFeedback::ConstPtr &feedback) { feedLabel.setText(QString::number(feedback->percent)); } void MainWindowClient::cancel() { handle.cancel(); }cpp_ros_action_server_class标准通讯
#include <QWidget> #include <actionlib/server/action_server.h> #include <demo_actions/CountNumberAction.h> #include <thread> #include <QVBoxLayout> #include <QPushButton> #include <QTableView> #include <iostream> #include <QStandardItemModel> MainWindowServer::MainWindowServer(ros::NodeHandle node, QWidget *parent) : QWidget(parent), aboredAllBtn("中止") { //创建server server = new actionlib::ActionServer<demo_actions::CountNumberAction>(node, "action_gui", boost::bind(&MainWindowServer::goal_cb, this, _1), boost::bind(&MainWindowServer::cancel_cb, this, _1), false); server->start(); /*-------------------------- 初始化ui --------------------------*/ setLayout(&layout); layout.addWidget(&aboredAllBtn); layout.addWidget(&tableView); //设置model tableView.setModel(&model); //表头 model.setHorizontalHeaderItem(0, new QStandardItem("ID")); model.setHorizontalHeaderItem(1, new QStandardItem("操作")); /*-------------------------- 绑定信号和槽 --------------------------*/ connect(this, &MainWindowServer::updateUI, this, &MainWindowServer::updateTableView); connect(&aboredAllBtn,&QPushButton::clicked,[this]{ //把所有的任务的isAborted变成true for (auto i = goals.begin(); i != goals.end(); ++i) { i->second.isAborted = true; } }); } void MainWindowServer::done_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle) { //获取当前任务状态,是否被取消 string id = goalHandle.getGoalID().id; long max = goalHandle.getGoal()->max; double duration = goalHandle.getGoal()->duration; ros::Rate rate(duration); //结果 demo_actions::CountNumberResult result; for (int i = 1; i < max; ++i) { /*-------------------------- 中断状态 --------------------------*/ if(goals[id].isAborted){ //中断 result.count = i; goalHandle.setAborted(result); //删除当前任务 goals.erase(id); // 再更新ui emit updateUI(); return; } /*-------------------------- 取消 --------------------------*/ if(goals[id].isCanceled){ result.count = i; goalHandle.setCanceled(result); //删除当前任务 goals.erase(id); // 再更新ui emit updateUI(); return; } //进度 demo_actions::CountNumberFeedback feedback; feedback.percent = (double) i / max; //回调进度 goalHandle.publishFeedback(feedback); //睡眠 rate.sleep(); } /*-------------------------- 成功状态 --------------------------*/ //成功 result.count = max; goalHandle.setSucceeded(result); //删除当前任务 goals.erase(id); // 再更新ui emit updateUI(); } void MainWindowServer::goal_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle) { ROS_INFO_STREAM("goal cb " << this_thread::get_id()); //1.决定是否要接受任务 goalHandle.setAccepted(); /*-------------------------- 添加到任务map中，更新界面 --------------------------*/ Goal goal; string id = goalHandle.getGoalID().id; goal.id = id; //添加到map goals.insert({id, goal}); //更新界面(主线程更新) 在主线程中更新 //发送更新信号 emit updateUI(); // updateTableView(); //开启线程处理 new std::thread(&MainWindowServer::done_cb, this, goalHandle); } //取消 void MainWindowServer::cancel_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle) { //获取任务id string id = goalHandle.getGoalID().id; //停下里任务,修改当前任务的isCanceled状态 goals[id].isCanceled = true; } MainWindowServer::~MainWindowServer() { } //子线程更新界面就会出现问题 void MainWindowServer::updateTableView() { //清理列表 model.clear(); //表头 model.setHorizontalHeaderItem(0, new QStandardItem("ID")); model.setHorizontalHeaderItem(1, new QStandardItem("操作")); int row = 0; //把所有的任务展示出来 for (auto i = goals.begin(); i != goals.end(); ++i) { string id = i->second.id; //添加一条数据 model.setItem(row, 0, new QStandardItem(QString(id.c_str()))); model.setItem(row, 1, new QStandardItem("")); //按钮 QPushButton *btn = new QPushButton("中止"); //中止点击事件 connect(btn,&QPushButton::clicked,[i]{ //修改中止状态 i->second.isAborted = true; }); //把后面的控件替换成按钮 tableView.setIndexWidget(model.index(row, 1), btn); ++row; } }param参数:
cpp_ros_param获取参数,外部传参
/*-------------------------- 设置param --------------------------*/ node.setParam("/heihei",123); /*-------------------------- param --------------------------*/ //获取所有的param //定义vector保存所有的param的名字 vector<string> names; string value; int a; node.getParam("heihei",a); ROS_INFO_STREAM("HEIHEI----"<<a); //node.getParam("/turtlesim/background_b",value); //ROS_INFO_STREAM("turtlesim key----------------- "<<value); //获取所有的param的名字 if (node.getParamNames(names)) { //获取每一个key:value for (string ele:names) { ROS_INFO_STREAM("names----"<<ele); //前提 需要知道当前数据类型 //定义value string value; if(node.getParam(ele,value)){ ROS_INFO_STREAM("key "<<ele<<" value "<<value); } }launch
<launch> <!-- 启动demo08.launch 文件 同时加载另外一个launch文件 --> <!--启动demo08.launch 全路径--> <!--<include file="$(find demo_config)/launch/demo08.launch"></include> <!--<node pkg="rviz" type="rviz" name="rviz" args="-d $(find demo_tf)/rviz/rviz.rviz">--></node>--> <!--<node pkg="demo_tf" type="turtle2_02_cpp" name="turtle3_cpp" output="screen" args="turtle3 up"></node>--> <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"></node> </launch>信号与曹:
cpp_qt_connect_class信号和槽绑定
//信号和槽绑定 connect(&clearBtn, &QPushButton::clicked, this, &MainWindow::clear);多线程:
cpp_thread_class开启多线程
#include <thread> //开启线程处理 new std::thread(&MainWindowServer::done_cb, this, goalHandle); void MainWindowServer::done_cb(actionlib::ServerGoalHandle<demo_actions::CountNumberAction> goalHandle) { }TF坐标转换:
cpp_ros_TF_TransformBroadcaster_main_TF坐标转换
#include <iostream> #include <ros/ros.h> #include <turtlesim/Pose.h> #include <tf/transform_broadcaster.h> using namespace std; void callBack(const turtlesim::Pose::ConstPtr &pose,tf::TransformBroadcaster *broadcaster){ // ROS_INFO_STREAM("receive turtle1 pose"); //发送的数据(位置和姿态) tf::Transform transform; //设置位置 transform.setOrigin(tf::Vector3{pose->x,pose->y,0}); //设置姿态 rx ry rz rpy（欧拉角） //四元数 可以表示姿态 (欧拉角可以和四元数相互转换) //四元数 tf::Quaternion quaternion; quaternion.setRPY(0,0,pose->theta); transform.setRotation(quaternion); //发送给tf broadcaster->sendTransform(tf::StampedTransform(transform,ros::Time().now(),"world","turtle1")); } int main(int argc,char *argv[]){ //节点名 string nodeName = "demo01_turtle1"; //初始化节点 ros::init(argc,argv,nodeName); //创建节点 ros::NodeHandle node; /*-------------------------- 创建broadcast --------------------------*/ tf::TransformBroadcaster broadcaster; /*-------------------------- 获取小乌龟1坐标 --------------------------*/ ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle1/pose", 5, boost::bind(callBack,_1,&broadcaster)); //事件轮询 ros::spin(); return 0; }cpp_ros_TF_TransformListener_main_TF坐标转换
#include <iostream> #include <ros/ros.h> #include <turtlesim/Spawn.h> #include <turtlesim/Pose.h> #include <tf/transform_broadcaster.h> #include <tf/transform_listener.h> using namespace std; void callBack(const turtlesim::Pose::ConstPtr &pose, tf::TransformBroadcaster *broadcaster) { // ROS_INFO_STREAM("receive turtle2 pose"); //发送的数据(位置和姿态) tf::Transform transform; //设置位置 transform.setOrigin(tf::Vector3{pose->x, pose->y, 0}); //设置姿态 rx ry rz rpy（欧拉角） //四元数 可以表示姿态 (欧拉角可以和四元数相互转换) //四元数 tf::Quaternion quaternion; quaternion.setRPY(0, 0, pose->theta); transform.setRotation(quaternion); //发送给tf broadcaster->sendTransform(tf::StampedTransform(transform, ros::Time().now(), "world", "turtle2")); } int main(int argc, char *argv[]) { //节点名 string nodeName = "demo01_turtle2"; //初始化节点 ros::init(argc, argv, nodeName); //创建节点 ros::NodeHandle node; /*-------------------------- asy --------------------------*/ ros::AsyncSpinner spinner(1); spinner.start(); /*-------------------------- 创建小乌龟 --------------------------*/ ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>("/spawn"); //等待服务 client.waitForExistence(); //数据 turtlesim::Spawn spawn; spawn.request.x = 3.0; spawn.request.y = 3.0; spawn.request.theta = 0.0; spawn.request.name = "turtle2"; //调用服务 client.call(spawn); /*-------------------------- 创建broadcast --------------------------*/ tf::TransformBroadcaster broadcaster; /*-------------------------- 获取小乌龟2坐标 --------------------------*/ ros::Subscriber subscriber = node.subscribe<turtlesim::Pose>("/turtle2/pose", 5, boost::bind(callBack, _1, &broadcaster)); ros::Publisher publisher = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 5); /*-------------------------- 获取小乌龟相对位置 --------------------------*/ tf::TransformListener listener; while (ros::ok()){ //接收结果 tf::StampedTransform transform; try { //参数1：最终表达的坐标系 //参数2：需要表示的坐标系 //参数3：时间戳 传(0)最近时间 //参数4：接收最终数据 listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform); //获取坐标 double x = transform.getOrigin().x(); double y = transform.getOrigin().y(); ROS_INFO_STREAM("x--------- "<<x<<" y------- "<<y); tf::Quaternion quaternion = transform.getRotation(); double z = quaternion.z(); geometry_msgs::Twist twist; double distance = sqrt(pow(x, 2) + pow(y, 2)); double angular = atan2(y, x); ///turtle1/cmd_vel 跟着乌龟１走 twist.linear.x=distance*0.5; twist.angular.z=angular*3; publisher.publish(twist); } catch (exception e) { // ROS_INFO_STREAM("exception"); } } //等待程序停止 // ros::waitForShutdown(); return 0; }cpp_ros_TF_TransformBroadcaster_TF坐标转换
#include <iostream> #include <ros/ros.h> #include <turtlesim/Pose.h> #include <tf/transform_broadcaster.h> using namespace std; /*--------------- 创建broadcast 需要在cmakeList.txt中导入 tf库 -----------------*/ tf::TransformBroadcaster broadcaster; //发送的数据(位置和姿态) tf::Transform transform; //设置位置 x y z double x= 1.0; // 位置坐标 double y = 5.0; double z = 0.0; transform.setOrigin(tf::Vector3{x,y,z}); //设置姿态 rx ry rz rpy（欧拉角） //四元数 可以表示姿态 (欧拉角可以和四元数相互转换) //四元数 z=ztheta角度 tf::Quaternion quaternion; double xtheta=0.0; //角度 double ytheta=0.0; double ztheta=2.0; quaternion.setRPY(xtheta,ytheta,ztheta); transform.setRotation(quaternion); //发送给tf world=事件坐标 名为turtle1的坐标 broadcaster.sendTransform(tf::StampedTransform(transform,ros::Time().now(),"world","turtle1"));cpp_ros_TF_TransformListener_TF坐标转换
#include <iostream> #include <ros/ros.h> #include <turtlesim/Spawn.h> #include <turtlesim/Pose.h> #include <tf/transform_broadcaster.h> #include <tf/transform_listener.h> using namespace std; //创建节点 ros::NodeHandle node; /*----------- 获取小乌龟相对位置 需要在cmakeList.txt中导入 tf库--------------------*/ ros::Publisher publisher = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 5); tf::TransformListener listener; while (ros::ok()){ //接收结果 tf::StampedTransform transform; try { //参数1：最终表达的坐标系 //参数2：需要表示的坐标系 //参数3：时间戳 传(0)最近时间 //参数4：接收最终数据 //获取已turtle2为坐标原点的坐标系中 turtle1的相对坐标,ros::Time(0)最近时间的坐标 listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform); //获取坐标 double x = transform.getOrigin().x(); double y = transform.getOrigin().y(); ROS_INFO_STREAM("x--------- "<<x<<" y------- "<<y); tf::Quaternion quaternion = transform.getRotation(); double z = quaternion.z(); geometry_msgs::Twist twist; double distance = sqrt(pow(x, 2) + pow(y, 2));//勾股定理 double angular = atan2(y, x);//取角度 ///turtle1/cmd_vel 跟着乌龟１走 twist.linear.x=distance*0.5;//线速度 twist.angular.z=angular*3;//角速度 publisher.publish(twist); } catch (exception e) { // ROS_INFO_STREAM("exception"); } }cpp_ros_pid
// 算离坐标点的的距离[自动扫地] float MainWindow::distance(float curx, float cury, float dstx, float dsty) { return sqrt(pow(curx - dstx, 2) + pow(cury - dsty, 2)); } /** * 计算需要转的角度 [自动扫地] * @param dstx * @param dsty * @param curx * @param cury * @param theta 小乌龟当前角度 * @return */ float MainWindow::caclAngle(float dstx,float dsty,float curx,float cury,float theta){ float angle = atan2(dsty-cury,dstx-curx)-theta; //角度范围在0-180 -180-0 if(angle>M_PI){ angle -= 2*M_PI; } else if (angle<-M_PI){ angle += 2*M_PI; } return angle; } /** * pid [自动扫地] * @param dstx * @param dsty * @param curx * @param cury */ //走的距离 float dst = distance(curx, cury, dstx, dsty); //走的时间 float time = 5.0; //调头时间 float turnTime = time/12; //频率 float hz = 10; //频率 ros::Rate rate(hz); //当前速度 float curSpeed = 0; //记录上一次误差 float lastError = 0; //总误差 float totalError = 0; //kp系数 float kp = kpEdit.text().toFloat(); //kd系数 float kd = kdEdit.text().toFloat(); //ki系数 float ki = kiEdit.text().toFloat(); geometry_msgs::Twist data; while (distance(curx, cury, dstx, dsty) > 0.1) { //目标速度 float tarSpeed = distance(curx, cury, dstx, dsty) / time; /*-------------------------- p --------------------------*/ //误差 float pError = tarSpeed - curSpeed; //调节速度 curSpeed += pError * kp; /*-------------------------- d --------------------------*/ float dError = pError - lastError; curSpeed += dError * kd; lastError = pError; /*-------------------------- i --------------------------*/ totalError += pError; curSpeed+=totalError*ki; //数据 data.linear.x = curSpeed; //角速度 data.angular.z = caclAngle(dstx,dsty,curx,cury,curTheta)/turnTime; }cpp_ros_distance_离坐标点的的距离_求向量
float MainWindow::distance(float curx, float cury, float dstx, float dsty) { return sqrt(pow(curx - dstx, 2) + pow(cury - dsty, 2)); }cpp_ros_caclAngle_计算需要转的角度
/** * 计算需要转的角度 [自动扫地] * @param dstx * @param dsty * @param curx * @param cury * @param theta 小乌龟当前角度 * @return */ float MainWindow::caclAngle(float dstx,float dsty,float curx,float cury,float theta){ float angle = atan2(dsty-cury,dstx-curx)-theta; //角度范围在0-180 -180-0 if(angle>M_PI){ angle -= 2*M_PI; } else if (angle<-M_PI){ angle += 2*M_PI; } return angle; }cpp_ros_Gripper_因时电动夹爪
#include "Gripper.h" // 需要在CMakeLists.txt中导入 rs232.cpp的库 add_executable(jaw main.cpp rs232.cpp Gripper.cpp) Gripper::Gripper(char *port,int btl):rs232(port,btl) { } Gripper::~Gripper() { } void Gripper::catchJaw(int speed, int power) { unsigned char data[10]; data[0] = 0xEB; data[1] = 0x90; data[2] = 0x01; data[3] = 0x05; data[4] = 0x10; data[5] = speed&0x00ff; data[6] = speed>>8; data[7] = power&0x00ff; data[8] = power>>8; data[9] = data[2]+data[3]+data[4]+data[5]+data[6]+data[7]+data[8]; rs232.Write(data,sizeof(data)/sizeof(unsigned char)); } void Gripper::release(int speed) { unsigned char data[8]; data[0] = 0xEB; data[1] = 0x90; data[2] = 0x01; data[3] = 0x03; data[4] = 0x11; data[5] = speed&0x00ff; data[6] = speed>>8; data[7] = data[2]+data[3]+data[4]+data[5]+data[6]; rs232.Write(data,sizeof(data)/sizeof(unsigned char)); }
urdf模拟仿真制作文件
urdf
<?xml version="1.0" encoding="UTF-8" ?> <robot name="box"> <!-- 把里面的中文 包括注释的中文 及这一条都删掉不然会报错--> <!-- 纹理 定义颜色 --> <material name="c1"> <color rgba="0.8 0.4 0 0.8"/> </material> <material name="c2"> <color rgba="0 0 1 0.8"/> </material> <!-- link --> <link name="base"> <visual> <geometry> <box size="0.2 0.2 0.6"/> </geometry> <material name="c1"/> </visual> </link> <!-- link --> <link name="link2"> <visual> <origin xyz="0 0 0.5"/> <geometry> <box size="0.3 0.3 1"/> </geometry> <material name="c2"/> </visual> </link> <!-- 连接点 fixed 固定 --> <joint name="joint1" type="fixed"> <origin xyz="0 0 0.3" rpy="0 0 0"/> <parent link="base"/> <child link="link2"/> </joint> <!-- 连接点 revolute 旋转--> <joint name="joint1" type="revolute"> <origin xyz="0 0 0.3" rpy="0 0 0"/> <parent link="base"/> <child link="link2"/> <!-- 0 1 0 对应 x y z ,表示按y轴旋转 --> <axis xyz="0 1 0"/> <!-- 限定范围 limit主要是限制child的旋转的范围。 lower和upper限制了旋转的弧度范围 effort限制的是转动过程中的受力范围.(正负value值，单位为牛或N) velocity限制了转动时的速度，单位为米/秒或m/s --> <limit effort="30" velocity="1.0" lower="-3.1415926" upper="3.1415926" /> </joint> <!-- 连接点 continuous旋转,没有旋转弧度限制可以一直旋转--> <joint name="joint1" type="continuous"> <origin xyz="0 0 0.3" rpy="0 0 0"/> <parent link="base"/> <child link="link2"/> <axis xyz="0 1 0"/> <!-- <limit effort="30" velocity="1.0" lower="-3.1415926" upper="3.1415926" />--> </joint> <!-- 连接点 prismatic 滑动--> <joint name="joint1" type="prismatic"> <origin xyz="0 0 0.3" rpy="0 0 0"/> <parent link="base"/> <child link="link2"/> <axis xyz="0 0 1"/> <limit effort="30" velocity="1.0" lower="-1.5" upper="1.5" /> </joint> </robot>
xacro模拟仿真制作文件
xacro
<?xml version="1.0" encoding="UTF-8" ?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="box"> <!-- 把里面的中文 包括注释的中文 及这一条都删掉不然会报错--> <!-- material 纹理 定义颜色 --> <xacro:macro name="MyColor" params="name"> <material name="${name}"> <xacro:if value="${name=='red'}"> <color rgba="1.0 0 0 1.0"/> </xacro:if> <xacro:if value="${name=='green'}"> <color rgba="0 1.0 0 1.0"/> </xacro:if> <xacro:if value="${name=='blue'}"> <color rgba="0 0 1.0 1.0"/> </xacro:if> <xacro:if value="${name=='white'}"> <color rgba="1.0 1.0 1.0 1.0"/> </xacro:if> </material> </xacro:macro> <!-- <geometry> --> <!-- size 用来描述长宽高(x, y, z) --> <box size="1 2 3"/> <!-- 圆柱体 半径和高度 单位米 --> <!--<cylinder length="0.6" radius="0.2" />--> <!-- 球体 半径单位米 --> <!--<sphere radius="1"/>--> <!-- 纹理文件 加载已经创建好的 3d模型文件--> <!-- <mesh filename="package://demo_urdf/meshes/bowl.dae"/>--> <!--<mesh filename="package://demo_urdf/meshes/cft.dae"/>--> <!-- </geometry> --> <!-- link连杆 ; name: 连杆名 ; colorname: 纹理颜色; xyz: 位置; --> <xacro:macro name="mylink" params="name size colorname xyz:='0 0 0'"> <link name="${name}"> <visual> <!-- 位置和姿态 位置 :xyz="0 0 0" 姿态 rpy是弧度值: 3.14/2=1.57/2=0.785 --> <!-- <origin xyz="0 0 0" rpy="0.785 0.785 0.785" /> --> <origin xyz="${xyz}" /> <geometry> <!-- size 用来描述长宽高(x, y, z) --> <box size="${size}"/> </geometry> <xacro:MyColor name="${colorname }"/> </visual> </link> </xacro:macro> <xacro:mylink name="base" size="0.2 0.2 0.6" color="red"/> <xacro:mylink name="link2" size="0.3 0.3 1" xyz="0 0 0.5" color="blue"/> <!-- 连接点 fixed 固定 --> <!-- 连接点 revolute 旋转--> <!-- 连接点 continuous旋转,没有旋转弧度限制可以一直旋转--> <!-- 连接点 prismatic 滑动--> <joint name="joint1" type="revolute"> <origin xyz="0 0 0.3" rpy="0 0 0"/> <parent link="base"/> <child link="link2"/> <!-- 0 1 0 对应 x y z ,表示按y轴旋转 --> <axis xyz="0 1 0"/> <!-- 限定范围 limit主要是限制child的旋转的范围。 lower和upper限制了旋转的弧度范围 effort限制的是转动过程中的受力范围.(正负value值，单位为牛或N) velocity限制了转动时的速度，单位为米/秒或m/s --> <limit effort="30" velocity="1.0" lower="-3.1415926" upper="3.1415926" /> </joint> </robot>
urdf与xacro模拟仿真启动配置launch文件
launch_ros_urdf_xacro模拟仿真启动配置
<launch> <arg name="model" default="geometry_box.urdf"/> <arg name="gui" default="true" /> <arg name="rvizconfig" default="$(find demo_urdf)/rviz/urdf.rviz" /> <param name="robot_description" command="$(find xacro)/xacro --inorder $(find demo_urdf)/urdf/$(arg model)" /> <param name="use_gui" value="$(arg gui)"/> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" /> </launch>启动: roslaunch demo_tf config.launch mode:=geometry_box.urdf 后面参数自己改,不传的话默认geometry_box.urdf
cpp_ros_ur3_aubo5_移动
#include <moveit/move_group_interface/move_group_interface.h> #include <geometry_msgs/Pose.h> moveit::planning_interface::MoveGroupInterface group("manipulator_i5");//aubo5 //moveit::planning_interface::MoveGroupInterface group("manipulator");//ur3 //设置目标 // group.setNamedTarget("home"); // 设置随机位置 // group.setRandomTarget(); //去到具体的位置 geometry_msgs::Pose pose; // 位置 pose.position.x = 0.3; pose.position.y = 0.4; pose.position.z = 0.2; // 姿态(四元素)(欧拉角转四元素) tf::Quaternion quat; quat.setRPY(deg2rad(0), deg2rad(90), deg2rad(90)); pose.orientation.x = quat.x(); pose.orientation.y = quat.y(); pose.orientation.z = quat.z(); pose.orientation.w = quat.w(); group.setPoseTarget(pose); //移动 group.move();cpp__ros_ur3_aubo5_轨迹规划
#include <moveit/move_group_interface/move_group_interface.h> #include <geometry_msgs/Pose.h> #include <tf/LinearMath/Quaternion.h> #include "moveit_visual_tools/moveit_visual_tools.h" moveit::planning_interface::MoveGroupInterface group("manipulator_i5");//aubo5 //moveit::planning_interface::MoveGroupInterface group("manipulator");//ur3 moveit::planning_interface::MoveGroupInterface::Plan plan; const moveit::planning_interface::MoveItErrorCode &code = group.plan(plan); if (code == moveit::planning_interface::MoveItErrorCode::SUCCESS) { // 成功 ROS_INFO_STREAM("计划成功: Success"); string frame = group.getPlanningFrame(); moveit_visual_tools::MoveItVisualTools tools(frame); const moveit::core::JointModelGroup *jointModelGroup = group.getCurrentState()->getJointModelGroup("manipulator"); tools.publishTrajectoryLine(plan.trajectory_, jointModelGroup); tools.trigger(); group.move(); } else { // 失败 ROS_INFO_STREAM("计划失败: Failed"); }cpp__ros_ur3_aubo5_断点调试_tools_promp
#include <moveit/move_group_interface/move_group_interface.h> #include <geometry_msgs/Pose.h> #include <tf/LinearMath/Quaternion.h> #include "moveit_visual_tools/moveit_visual_tools.h" moveit::planning_interface::MoveGroupInterface group("manipulator_i5");//subo5 //moveit::planning_interface::MoveGroupInterface group("manipulator");//ur3 moveit_visual_tools::MoveItVisualTools tools(group.getPlanningFrame()); tools.prompt("start move home"); moveHome();cpp__ros_ur3_aubo5_障碍物环境
//moveit环境引入 #include <moveit/move_group_interface/move_group_interface.h> #include <geometry_msgs/Pose.h> #include <tf/LinearMath/Quaternion.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> moveit::planning_interface::MoveGroupInterface group("manipulator_i5"); //moveit::planning_interface::MoveGroupInterface group("manipulator");//ur3 //场景管理器 moveit::planning_interface::PlanningSceneInterface scene; //障碍物集合 std::vector<moveit_msgs::CollisionObject> objects; //单个障碍物 moveit_msgs::CollisionObject obj; //操作类型 obj.operation = obj.ADD; obj.id = "box"; obj.header.frame_id = group.getPlanningFrame();//坐标系 //形状 shape_msgs::SolidPrimitive shape; shape.type = shape.BOX;//盒子形状 shape.dimensions.push_back(1); shape.dimensions.push_back(2); shape.dimensions.push_back(3); obj.primitives.push_back(shape); //位置和姿态 geometry_msgs::Pose pose; pose.position.x = 0.1; pose.position.y = 0.2; pose.position.z = 0.3; // 姿态(四元素)(欧拉角转四元素) tf::Quaternion quat; quat.setRPY(deg2rad(0), deg2rad(0), deg2rad(0)); pose.orientation.x = quat.x(); pose.orientation.y = quat.y(); pose.orientation.z = quat.z(); pose.orientation.w = quat.w(); obj.primitive_poses.push_back(pose); objects.push_back(obj); std::vector<moveit_msgs::ObjectColor> colors; moveit_msgs::ObjectColor color; color.id = "box"; // [0,255] int, float32 [0, 1] color.color.r = 255 / 255.0; color.color.g = 0; color.color.b = 0; color.color.a = 1; colors.push_back(color); //在场景中添加障碍物 // scene.addCollisionObjects(objects); scene.applyCollisionObjects(objects, colors);cpp__ros_ur3_aubo5_位置和姿态欧拉角转四元素
#include <tf/LinearMath/Quaternion.h> double deg2rad(double deg) { return deg * M_PI / 180.0; } //位置和姿态 geometry_msgs::Pose pose; pose.position.x = 0.1; pose.position.y = 0.2; pose.position.z = 0.3; // 姿态(四元素)(欧拉角转四元素) tf::Quaternion quat; quat.setRPY(deg2rad(0), deg2rad(0), deg2rad(0)); pose.orientation.x = quat.x(); pose.orientation.y = quat.y(); pose.orientation.z = quat.z(); pose.orientation.w = quat.w(); obj.primitive_poses.push_back(pose);opencv
estimateRigidTransform()：计算多个二维点对或者图像之间的最优仿射变换矩阵 （2行x3列），H可以是部分自由度，比如各向一致的切变。 getAffineTransform()：计算3个二维点对之间的仿射变换矩阵H（2行x3列），自由度为6. warpAffine()：对输入图像进行仿射变换 findHomography： 计算多个二维点对之间的最优单映射变换矩阵 H（3行x3列） ，使用最小均方误差或者RANSAC方法 。 getPerspectiveTransform()：计算4个二维点对之间的透射变换矩阵 H（3行x3列） warpPerspective()： 对输入图像进行透射变换 perspectiveTransform()：对二维或者三维矢量进行透射变换，也就是对输入二维坐标点或者三维坐标点进行投射变换。 estimateAffine3D：计算多个三维点对之间的最优三维仿射变换矩阵H （3行x4列） transform()：对输入的N维矢量进行变换，可用于进行仿射变换、图像色彩变换. findFundamentalMat：计算多个点对之间的基矩阵H。cpp_opencv_convertTo数据类型转换 --------------------------
Mat data = src.reshape(src.channels(),src.rows*src.cols); data.convertTo(data,CV_32FC3);cpp_opencv_zeros创建空白的图片
// 创建一张空白的图片 // CV_8UC3 ----> CV_32FC3 //Mat data = Mat::zeros(src.rows*src.cols,1,CV_32FC3); // 创建一张空白的图片， 用于接收kmeans聚类结果 //Mat distImg(src.size(),src.type()); //Mat labelimg = Mat::zeros(src.size(),CV_8UC3);cpp_opencv_reshape数据打扁
// 数据打扁 Mat data = src.reshape(src.channels(),src.rows*src.cols); //data.convertTo(data,CV_32FC3);//看情况是否需要转换类型 CV_8UC3 ----> CV_32FC3 /* 方式2 Mat src = imread("../img/zhengjianzhao0.jpg"); cout<<"src.size(): "<<src.size()<<endl; Mat data = Mat::zeros(src.rows*src.cols,1,CV_32FC3); //填充数据 for (int i = 0; i < src.rows; ++i) { for (int j = 0; j < src.cols; ++j) { Vec3b color = src.at<Vec3b>(i,j); int index=i*src.cols+j; data.at<Vec3f>(index)=color; } } */cpp_opencv_RNG随机数
RNG rng(12345678); Vec3b colors[]={ Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), };cpp_opencv_kmeans替换背景
#include <opencv2/opencv.hpp> #include <iostream> using namespace cv; using namespace std; /** * * 替换背景 */ int main(){ Mat src = imread("../img/zhengjianzhao0.jpg"); imshow("src0",src); // 创建一张空白的图片 // CV_8UC3 ----> CV_32FC3 //Mat data = Mat::zeros(src.rows*src.cols,1,CV_32FC3); // 将数据打扁 写法1 /*for (int row = 0; row < src.rows; ++row) { for (int col = 0; col < src.cols; ++col) { // 获取每一个像素点的颜色 Vec3b color = src.at<Vec3b>(row,col); // 将数据封装到Data中 int index = row*src.cols +; data.at<Vec3f>(index) = color; } }*/ // 将数据打扁 写法2 Mat data = src.reshape(src.channels(),src.rows*src.cols); // CV_8UC3 ----> CV_32FC3 data.convertTo(data,CV_32FC3); int k = 4; // 分几类 Mat labels; // 输出的结果 对数据打标签 // 终止条件TermCriteria 参数1:类型 COUNT根据次数来停止, EPS根据预值来停止,COUNT +EPS 次数+预值; //参数2:最大次数 参数3:预值 (停止的方式可以是最大次数,可以是预值,具体看参数1) TermCriteria termCriteria(TermCriteria::COUNT + TermCriteria::EPS,10,0.1); Mat centers; // K-means算法; 参数1:data: 输入的数据 参数2:k :分几类 参数3:labels:输出的结果,对数据打标签 参数4:termCriteria:终止条件 //参数4:执行几次kmeans算法 参数5: 选择中心点 参数6:center中心有一个输出 kmeans(data,k,labels,termCriteria,3,KMEANS_RANDOM_CENTERS,centers); // ---------start----------- 根据labels绘制图像 预显示 分类颜色填充的图片 Mat labelimg = Mat::zeros(src.size(),CV_8UC3); RNG rng(12345678); Vec3b colors[]={ Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), }; for (int row = 0; row < labelimg.rows; ++row) { for (int col = 0; col < labelimg.cols; ++col) { int index = row*labelimg.cols + col; // 获取当前位置对应的标签 int label = labels.at<int>(index); // 根据label绘制不同的颜色 labelimg.at<Vec3b>(row,col) = colors[label]; } } // ---------end----------- 预显示 分类颜色填充的图片 imshow("labelimg",labelimg); // 获取当前的背景 Vec3b backgroundColor = labelimg.at<Vec3b>(0,0); // 获取背景对应的标签 int backgroundlabel = labels.at<int>(0); // 修改原图 for (int row = 0; row < src.rows; ++row) { for (int col = 0; col < src.cols; ++col) { // 根据当前位置标签来修改背景颜色 int currentLabel = labels.at<int>(row*src.cols+col); // 判断当前的label是否等于backgroundLabel if(currentLabel == backgroundlabel){ src.at<Vec3b>(row,col) = Vec3b(255,255,0); } } } imshow("src",src); waitKey(); return 0; }cpp_opencv_distanceTransform距离变换
#include <opencv2/opencv.hpp> #include <iostream> using namespace cv; using namespace std; /** * * @return */ /* 计算距离的方法 DIST_USER User defined distance. DIST_L1 distance = |x1-x2| + |y1-y2| DIST_L2 the simple euclidean distance DIST_C distance = max(|x1-x2|,|y1-y2|) DIST_L12 L1-L2 metric: distance = 2(sqrt(1+x*x/2) - 1)) DIST_FAIR distance = c^2(|x|/c-log(1+|x|/c)), c = 1.3998 DIST_WELSCH distance = c^2/2(1-exp(-(x/c)^2)), c = 2.9846 DIST_HUBER distance = |x|<c ? x^2/2 : c(|x|-c/2), c=1.345 */ int main(){ Mat src = imread("../img/shape.jpg"); // 彩图 --》 灰度图 Mat gray; cvtColor(src,gray,COLOR_BGR2GRAY); Mat binary; threshold(gray,binary,0,255,THRESH_BINARY_INV|THRESH_TRIANGLE); imshow("binary",binary); Mat distanceResult; //参数1: 输入图像 参数2: 输出图像 参数3: 计算距离的方法 参数4: 掩膜的大小，一般为3x3 或者 5x5 ; 参数5: dstType = CV_32F 选用默认值即可 distanceTransform(binary,distanceResult,DIST_L2,3); // 将数据进行归一化 0,~1 Mat normalResult; normalize(distanceResult,normalResult,0,1.0,NORM_MINMAX); cout<<normalResult<<endl; // 0 ~1.0 自动显示 0-255 imshow("distance",normalResult); imshow("src",src); waitKey(); return 0; }cpp_opencv_resize缩放
Mat src = imread("../img/02_qiebian.jpg",IMREAD_COLOR); Mat mat1; resize(src,mat1,Size(src.cols/2,src.rows/2)); imshow("mat1",mat1);cpp_opencv_warpAffine仿射_旋转
Mat src = imread("../img/02_qiebian.jpg",IMREAD_COLOR); Point2f center(src.cols/2,src.rows/2); // 定义仿射矩阵： 参数1：中心点， 参数2：旋转角度 90度，参数3：缩放系数 Mat matrix =getRotationMatrix2D(Point2f(src.cols/2,src.rows/2),90,1); Mat out; //1.原始图像,2.变换矩阵,3.(高度,宽度) warpAffine(src,out,matrix,src.size()); imshow("out",out);cpp_opencv_warpAffine仿射_平移
Mat src = imread("../img/02_qiebian.jpg",IMREAD_COLOR); # 定义位移变换矩阵 Mat matrixShif =(Mat_<double>(2,3)<<1,0,100,0,1,100); cout<<"matrixShif = "<<matrixShif<<endl; /*matrixShif = [1, 0, 100; 0, 1, 100] */ Mat dst; //1.原始图像,2.变换矩阵,3.(高度,宽度) warpAffine(src,dst,matrixShif,src.size()); imshow("dst",dst);cpp_opencv_Mat_matrix自定义卷积核矩阵
# 定义变换矩阵 // 定义卷积核 // Mat kernel = (Mat_<char>(3,3)<<-1,-1,-1,-1,9,-1,-1,-1,-1); Mat matrixShift =(Mat_<double>(2,3)<<1,0,100,0,1,100); cout<<"m = "<<endl<<m<<endl<<endl; /*m = [1, 0, 100; 0, 1, 100]*/cpp_opencv_erode腐蚀
// 腐蚀去噪点 Mat erodesrc; //返回图形 //MORPH_RECT 矩形, MORPH_ELLIPSE 椭圆形, MORPH_CROSS 十字型 Mat kernel = getStructuringElement(MORPH_RECT,Size(5,5)); erode(thresrc,erodesrc,kernel); imshow("erodesrc",erodesrc);cpp_opencv_dilate_膨胀
// 膨胀去噪点 Mat erodesrc; //返回图形 //MORPH_RECT 矩形, MORPH_ELLIPSE 椭圆形, MORPH_CROSS 十字型 Mat kernel = getStructuringElement(MORPH_RECT,Size(5,5)); dilate(thresrc,erodesrc,kernel); imshow("erodesrc",erodesrc);cpp_opencv_morphologyEx_开操作
// 开操作 Mat dst; Mat kernel = getStructuringElement(MORPH_RECT,Size(5,5)); morphologyEx(src,dst,MORPH_OPEN,kernel);cpp_opencv_morphologyEx_闭操作
// 闭操作 Mat dst; Mat kernel = getStructuringElement(MORPH_RECT,Size(5,5)); morphologyEx(src,dst,MORPH_CLOSE,kernel);cpp_opencv_HoughLinesP霍夫直线
// 霍夫直线 vector<Vec4i> lines; //参数１：图 参数２：返回的线段点 参数３：线段以像素为单位的距离精度，double类型的，推荐用1.0 参数３：// 线段以弧度为单位的角度精度，推荐用CV_PI/180 // 参数５：累加平面的阈值参数，int类型，超过设定阈值才被检测出线段，值越大，基本上意味着检出的线段越长，检出的线段个数越少。 HoughLinesP(dilateSrc,lines,1,CV_PI/180,30,20,10); for (int i = 0; i < lines.size(); ++i) { Vec4i li = lines[i]; int x1=li[0]; int y1=li[1]; int x2=li[2]; int y2=li[3]; cout<<x1<<endl; //绘制线段 line(src,Point(x1,y1),Point(x2,y2),Scalar(255,0,0),2); }cpp_opencv_line绘制线段
Mat src = imread("../img/engline.jpg",IMREAD_COLOR); //绘制线段 line(src,Point(x1,y1),Point(x2,y2),Scalar(255,0,0),2);cpp_opencv_HoughCircles霍夫圆
/* 参数1: grayImg 8-bit 单通道图片 参数2: circles Vec3f 返回圆的参数 参数3: method 检测方法, 当前只有HOUGH_GRADIENT 参数4: dp 累加器分辨率和图像分辨率的反比例, 例如: 如果 dp=1 累加器和输入图像分辨率相同. 如果 dp=2 累加器宽高各为输入图像宽高的一半相同. 参数5: minDist 检测到圆的圆心之间的最小距离。 如果参数太小，除了真实的一个之外，可能错误地检测到多个相邻的圆圈。 如果参数太大，可能会遗漏一些圆 参数6: 100 它是两个传递给Canny边缘检测器的较高阈值（较低的阈值是此值的一半） 参数7:, 它是检测阶段圆心的累加器阈值。 它越小，会检测到更多的假圆圈。较大累加器值对应的圆圈将首先返回。 参数8: minRadius 最小圆半径. 参数9: maxRadius 最大圆半径. 如果<=0, 会使用图片最大像素值 如果< 0, 直接返回圆心, 不计算半径 */ vector<Vec3f> circles; HoughCircles(grayImg, circles, HOUGH_GRADIENT, 1, 10, 100, 30, 5, 30); for (int i = 0; i < circles.size(); ++i) { Vec3f c = circles.at(i); Point center(c[0], c[1]); float radius = c[2]; cout << radius << endl; //画圆 参数1:图 参数2:中心点 参数3:半径 参数4:颜色 参数5:线条宽度 参数6:抗锯齿 circle(frame, center, radius, Scalar(0, 255, 0), 2, LINE_AA); } imshow("rest", frame);cpp_opencv_circle画圆
for (int i = 0; i < circles.size(); ++i) { Vec3f c = circles.at(i); Point center(c[0], c[1]); float radius = c[2]; cout << radius << endl; //画圆 参数1:图 参数2:中心点 参数3:半径 参数4:颜色 参数5:线条宽度 参数6:抗锯齿 circle(frame, center, radius, Scalar(0, 255, 0), 2, LINE_AA); }cpp_opencv_findContours查找轮廓
std::vector<std::vector<cv::Point> > Contours; findContours(thresOutsrc,Contours,cv::RetrievalModes::RETR_EXTERNAL,cv::ContourApproximationModes::CHAIN_APPROX_SIMPLE); double maxarea=0; int y=0; cout<<"Contours.size()"<<Contours.size()<<endl; for (int i = 0; i < Contours.size(); ++i) { double area = contourArea(Contours[i]); if(area>maxarea){ maxarea=area; y=i; } } Rect rect = boundingRect(Contours[y]); //drawContours(src,Contours,y,Scalar(255,255,0),1); Mat m=src(rect);cpp_opencv_warpPerspective透视变换
/** 图片的透视变换 # 左上角 pt1 = (120,90) # 右上角 pt2 = (350,90) # 左下角 pt3 = (60,470) # 右下角 pt4 = (430,470) */ #include <iostream> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; int main(int argc,char** argv){ string path = "/home/kaijun/Pictures/zhengjianzhao.png"; Mat src = imread(path,IMREAD_COLOR); imshow("src",src); vector<Point2f> sourcePoint={Point2f(120,90),Point2f(350,90),Point2f(60,470),Point2f(430,470)}; // 处理完成之后，将图像映射到新的空间中去 vector<Point2f> dstPoint = {Point2f(0,0),Point2f(480,0),Point2f(0,600),Point2f(480,600)}; // 计算透视变换的转换矩阵 Mat matrix = getPerspectiveTransform(sourcePoint,dstPoint); // 调用转换函数 Mat dst; warpPerspective(src,dst,matrix,Size(480,600)); imshow("dst",dst); waitKey(0); return 0; }cpp_opencv_equalizeHist直方图均衡化
Mat src = imread("../img/itheima.jpg"); vector<Mat> srcChannels; // 切割 ， 将BGR 拆成 单个通道 split(src,srcChannels); Mat channel0; Mat channel1; Mat channel2; equalizeHist(srcChannels[0],channel0); equalizeHist(srcChannels[1],channel1); equalizeHist(srcChannels[2],channel2); vector<Mat> resChannels = {channel0,channel1,channel2}; Mat dst; // 合并BGR merge(resChannels,dst); imshow("src",src); imshow("dst",dst);cpp_opencv_matchTemplate模板匹配
// 读取原图 Mat src = imread("../assets/zhaonimei.jpg",IMREAD_COLOR); imshow("src",src); // 模板 Mat temp = imread("../assets/mei.jpg",IMREAD_COLOR); imshow("teamplate",temp); // 调用api查找内容 int width = src.cols - temp.cols; int height = src.rows - temp.rows; Mat result(height,width,CV_8UC1); matchTemplate(src,temp,result,TM_SQDIFF); // 将结果进行归一化处理 , 可做可不做 normalize(result,result,0,1,NORM_MINMAX); Point minLoc,maxLoc; double min,max; // 求最大值最小值 minMaxLoc(result,&min,&max,&minLoc,&maxLoc); printf("min:%f ,max:%f..\n",min,max); //画矩形 rectangle(src,Rect(minLoc.x,minLoc.y,temp.cols,temp.rows),Scalar(0,0,255),2,LINE_AA); imshow("dst",src);cpp_opencv_rectangle画矩形
//画矩形 rectangle(src,Rect(minLoc.x,minLoc.y,temp.cols,temp.rows),Scalar(0,0,255),2,LINE_AA);cpp_opencv_filter2D图像的卷积
#include <iostream> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; int main(){ Mat img = imread("../assets/lena.jpg",IMREAD_COLOR); imshow("src",img); // 定义卷积核 Mat kernel = (Mat_<char>(3,3)<<-1,-1,-1, -1,9,-1, -1,-1,-1); // 进行图像的卷积操作 Mat dst; filter2D(img,dst,-1,kernel); imshow("dst",dst); waitKey(0); destroyAllWindows(); return 0; }cpp_opencv_GaussianBlur高斯模糊
Mat gray; //返回值 cvtColor(src_img,gray,COLOR_BGR2GRAY); // 高斯模糊 参数1:图像 参数3:卷积核大小, 参数4:标准差越大，去除高斯噪声能力越强，图像越模糊 GaussianBlur(gray,gray,Size(3,3),0);
cpp_opencv_Scharr算子
Scharr(gray,xgray,CV_16S,1,0);cpp_opencv_findContours轮廓提取_比较全
#include <iostream> #include <opencv2/opencv.hpp> #include <stdio.h> using namespace std; using namespace cv; Mat img; void changeThresh(int val,void* userdata){ // 1. 定义结果矩阵 Mat cannyImg; // 使用Canny算法获取图像的边缘信息 Canny(img,cannyImg,50,val); // 调用api从canny图像上查找轮廓 vector<vector<Point>> contours; vector<Vec4i> hierachy; findContours(cannyImg,contours,hierachy,RETR_EXTERNAL,CHAIN_APPROX_NONE); Mat dst = Mat::zeros(img.size(), img.type()); drawContours(dst,contours,-1,Scalar(255,255,0),2,LINE_AA); // 绘制轮廓 for(int i=0;i<contours.size();i++){ vector<Point> c = contours[i]; // 计算面积 double area = contourArea(c); cout<<"面积为："<<area<<endl; // 计算弧长 double arc = arcLength(c,true); cout<<"弧长为："<<endl; // 计算外切圆 Point2f center; float radius; minEnclosingCircle(c,center,radius); cout<<"圆心坐标："<<center<<" 半径："<<radius; circle(dst,center,radius,Scalar(0,0,255),2,LINE_AA); } imshow("dst",dst); int i = *static_cast<int *>(userdata); cout<<val<<"======"<<(i)<<endl; } int main(){ img = imread("../assets/shape.jpg", IMREAD_COLOR); imshow("src",img); int thresh=104; int userdata=100; cout<<userdata<<"=="<<&userdata<<endl; namedWindow("dst"); createTrackbar("thresh","dst",&thresh,255,changeThresh,&userdata); changeThresh(104,&userdata); // imshow("dst",img); waitKey(0); destroyAllWindows(); return 0; }cpp_opencv_floodFill泛洪填充/漫水填充
/* int cv::floodFill ( InputOutputArray image, 输入图像 Point seedPoint, 种子点位置 Scalar newVal, 填充颜色值 Rect * rect = 0, 输出填充区域的boundingbox Scalar loDiff = Scalar(), 颜色的最小差异 Scalar upDiff = Scalar(), 颜色的最大差异 int flags = FLOODFILL_FIXED_RANGE 常用的是按照颜色差异范围来填充 ) */ #include <iostream> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; Mat src; RNG rng(123456); void onMouse(int event, int x, int y, int flags, void* userdata){ // 如果鼠标左键被按下，就在这里打个点开始填充 if(event == CV_EVENT_LBUTTONDOWN){ // 构建一个随机的颜色 Scalar randomColor = Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)); Rect rect; // 输入图 种子点 填充颜色 输出填充大小 最小差异 最大差异 按照范围填充 floodFill(src,Point(x,y),randomColor,&rect,Scalar(20,20,20),Scalar(100,100,100),FLOODFILL_FIXED_RANGE); cout<<rect.size()<<endl; imshow("src",src); } } int main(int argc,char** argv){ string path = "/home/kaijun/Documents/resource/opencv/zp.jpg"; // 读取图片 src = imread(path,IMREAD_COLOR); // 定义窗口 namedWindow("src",WINDOW_NORMAL); setMouseCallback("src",onMouse); imshow("src",src); waitKey(0); return 0; }cpp_opencv_watershed图像分水岭
// // Created by itcast on 7/22/20. // #include <opencv2/opencv.hpp> #include <iostream> using namespace cv; using namespace std; /** * 图像分水岭 */ Mat src = imread("../img/wan.png"); Mat markers = Mat::zeros(src.rows,src.cols,CV_32SC1); // 只是为了显示markers的标记 Mat img = Mat::zeros(src.rows,src.cols,CV_8UC1); int n = 1; RNG rng(123451325); void onMouse(int event, int x, int y, int flags, void* userdata){ // 如果用户按下左键 就给makers打标记 if(event == EVENT_LBUTTONDOWN){ // 打的是标记 circle(markers,Point(x,y),50,Scalar(n++),-1); // 显示颜色 circle(img,Point(x,y),50,Scalar(255),-1); imshow("img",img); }else if(event == EVENT_RBUTTONDOWN){ // 执行分水岭算法 watershed(src,markers); cout<<"markers:"<<markers<<endl; vector<Vec3b> colors = { Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), Vec3b(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)), }; // 展现最终分割结果 Mat dst = Mat::zeros(src.rows,src.cols,CV_8UC3); for (int row = 0; row < markers.rows; ++row) { for (int col = 0; col < markers.cols; ++col) { int label = markers.at<int>(row,col); if(label>0 && label < n){ dst.at<Vec3b>(row,col) = colors[label]; }else{ dst.at<Vec3b>(row,col) = Vec3b(0,0,255); } } } imshow("dst",dst); } } int main(){ imshow("src",src); setMouseCallback("src",onMouse); waitKey(); return 0; }cpp_time时间格式化
time_t tm; time(&tm); struct tm *t2 = localtime(&tm); char buf[1024]; strftime(buf, sizeof(buf), "%c", t2);cpp_opencv_VideoCapture视频处理
vector<Mat> frames;//储存图片 VideoCapture video(0); //从摄像头获取数据 //VideoCapture video("./data/twotiger.avi"); //从视频文件读取数据 bool isOpened = video.isOpened(); if (isOpened) { cout << "视频是否打开成功：" << isOpened << endl; // 获取图片的信息:帧率 double fps = video.get(CAP_PROP_FPS); //获取每帧宽度 double width = video.get(CAP_PROP_FRAME_WIDTH); //获取每帧的高度 double height = video.get(CAP_PROP_FRAME_HEIGHT); cout << "帧率：：" << fps << endl; cout << "宽度：" << width << endl; cout << "高度" << height << endl; //从视频中读取8帧信息 int count = 0; while (count < 8) { count = count + 1; // 读取成功or失败， 当前帧数据 Mat frame; bool flag = video.read(frame); //将图片信息写入到文件中 if (flag) { frames.push_back(frame); } cout << "图片截取完成啦" << endl; } }cpp_opencv_findChessboardCorners相机标定查找及保存角点
/*查找及保存角点 方格图*/ /*视频读取图片*/ /*vector<Mat> frames;//储存图片 VideoCapture video(0); //从摄像头获取数据 //VideoCapture video("./data/twotiger.avi"); //从视频文件读取数据 bool isOpened = video.isOpened(); if (isOpened) { cout << "视频是否打开成功：" << isOpened << endl; // 获取图片的信息:帧率 double fps = video.get(CAP_PROP_FPS); //获取每帧宽度 double width = video.get(CAP_PROP_FRAME_WIDTH); //获取每帧的高度 double height = video.get(CAP_PROP_FRAME_HEIGHT); cout << "帧率：：" << fps << endl; cout << "宽度：" << width << endl; cout << "高度" << height << endl; //从视频中读取8帧信息 int count = 0; while (count < 8) { count = count + 1; // 读取成功or失败， 当前帧数据 Mat frame; bool flag = video.read(frame); //将图片信息写入到文件中 if (flag) { frames.push_back(frame); } cout << "图片截取完成啦" << endl; } }*/ //储存所有图片路径 vector<String> filePaths; //加载所有图片路径 cv::glob("./data/image_*.jpg", filePaths); Size patternsize(6, 9);//图片角点 方格与方格的角点 int square_size = 20;//方格的尺寸 Size imageSize;//图片大小 vector<vector<Point3f>> objectPoints; vector<vector<Point2f>> imagePoints; //遍历图片路径，读取所有图片 for (String filePath:filePaths) { //读取图片 Mat image = imread(filePath, IMREAD_COLOR); cout << image.size() << endl; Mat img, gray;//复制图片 ，及创建灰度图 image.copyTo(img); imageSize = img.size(); // 创建灰度图 cvtColor(img, gray, COLOR_BGR2GRAY); vector<Point2f> corners; //找到的角点 this will be filled by the detected corners //找到这些图片的角点，（（绘制）2D //参数１：图片的灰度图 参数２：patternsize 图片的角点 [宽(列)，高(行)] 参数3：corners 找到的角点 参数４：都加上，CALIB_CB_ADAPTIVE_THRESH ， CALIB_CB_FAST_CHECK:快速图片检测 bool patternfound = findChessboardCorners(gray, patternsize, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK); if (patternfound)//优化查找的结果 官方代码 cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); //画上角点 //参数１：图片的灰度图 参数２：patternsize 图片的角点 [宽(列)，高(行)] 参数3：corners 找到的角点 参数４：findChessboardCorners查找角点的返回值 drawChessboardCorners(img, patternsize, corners, patternfound); //生成对应的３ｄ坐标 vector<Point3f> obj_points; /** * x,y,z (列，行，ｚ) * [0, 0, 0] [1, 0, 0], [2, 0, 0] ... [5, 0, 0] * [0, 1, 0] [1, 1, 0], [2, 1, 0] ... [5, 1, 0] * ... * [0, 8, 0] [1, 8, 0], [2, 8, 0] ... [5, 8, 0] */ // 高度 for (int i = 0; i < patternsize.height; ++i) { // 宽度 for (int j = 0; j < patternsize.width; ++j) { // obj_points.push_back(Point3f(j* square_size,i* square_size,0)); //square_size 方格的尺寸 obj_points.emplace_back(j * square_size, i * square_size, 0); } } objectPoints.push_back(obj_points);//再次封装３ｄ坐标 格式：vector<vector<Point3f>> objectPoints; imagePoints.push_back(corners); // 再次封装找到的角点 格式：vector<vector<Point2f>> imagePoints; // imshow("img",img); // waitKey(0); } cv::destroyAllWindows(); Mat cameraMatrix; // 相机内参 Mat distCoeffs; // 畸变系数 Mat rvecs, tvecs; // 标定板旋转，平移姿态 // * 4. 执行标定 -> 相机内参+畸变系数 //参数１：３ｄ坐标 参数２：３ｄ坐标找到的角点 参数３：图片大小 参数４：相机内参 参数５：畸变系数 参数６：rvecs标定板旋转 参数７：tvecs平移姿态 double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs); std::cout << "cameraMatrix相机内参: \n" << cameraMatrix << std::endl; std::cout << "distCoeffs畸变系数: \n" << distCoeffs << std::endl; std::cout << "rms: \n" << rms << std::endl; // * 5. 保存这些内参 time_t tm; time(&tm); struct tm *t2 = localtime(&tm); char buf[1024]; strftime(buf, sizeof(buf), "%c", t2); // 图片宽高，标定版数量，标定版尺寸，格子大小，时间 // FileStorage fs("./calibration.xml", FileStorage::WRITE); FileStorage fs("./calibration.yml", FileStorage::WRITE); fs << "time" << buf;//时间标定版尺寸 fs << "rms" << rms; fs << "board_count" << (int) filePaths.size();//图片数量 fs << "board_size" << patternsize; // 标定版尺寸 fs << "board_square" << square_size; // 格子大小 fs << "image_width" << imageSize.width;//图片宽带 fs << "image_height" << imageSize.height;//图片高度 fs << "cameraMatrix" << cameraMatrix;//相机内参 fs << "distCoeffs" << distCoeffs;//畸变系数 fs.release();cpp_opencv_CommandLineParser获取键盘输入的参数
int main(int argc, char **argv) { //获取键盘输入的参数 cv::CommandLineParser parser(argc, argv, "{@arg1||}{@arg2||}{@arg3||}{@arg4|15|}" "{scale s|1.0|}{flip f||}{help h||}");//导入opencv2 if (parser.has("h")) {//从键盘获取 判断用户是否输入的h值 printHelp(); return 0; } int board_width = parser.get<int>(0);//从键盘获取用户输入的第一位数字 int board_height = parser.get<int>(1);//从键盘获取用户输入的第2位数字 int square_size = parser.get<int>(2);//从键盘获取用户输入的第3位数字 int board_num = parser.get<int>(3); //从键盘获取用户输入的第3位数字；ps：最大图片采集数量 float image_scale = parser.get<float>("scale");//从键盘获取用户输入的scale 的值 bool flipHorizontal = parser.has("f");//从键盘获取用户输入的f或flip 的值 }cpp_opencv_glob获取路径下的所有文件
// 图片像素尺寸 Size imgSize; // 图片路径 cv::String src_path = "./data/image_*.jpg"; std::vector<String> filenames; cv::glob(src_path, filenames);//获取路径下所有文件名 cout << "filenames.size:" << filenames.size() << endl; for (auto& imgName : filenames) { // 读取图片 Mat img = imread(imgName, IMREAD_COLOR); // 获取图片像素尺寸 imgSize = img.size(); imshow("src",img); std::cout << "name: " << imgName<< " imgSize: " << imgSize << std::endl; //... waitKey(0); }cpp_opencv_findContours_minAreaRect_getRotationMatrix2D查找轮廓_外切矩形_按最小矩形旋转
// 查找轮廓 vector<vector<Vec2i>> contours; vector<Vec4i> hierarchy; findContours(binary,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE); RotatedRect resultRect; // 遍历轮廓，求轮廓的最大外切矩形 for(int i = 0; i<contours.size();i++){ // 取到当前遍历轮廓 RotatedRect rect = minAreaRect(contours[i]); if(rect.size.width > img.size().width*0.75){ // 绘制轮廓 //drawContours(img,contours,i,Scalar(255,0,255),2,LINE_AA); // 记录外切矩形 resultRect = rect; } } // 打印图片需要旋转的角度 cout<<resultRect.angle<<endl; // 先对图片进行旋转 Point2f center(img.cols/2,img.rows/2); Mat matrix = getRotationMatrix2D(center,(resultRect.angle)+90,1); warpAffine(img,outputImg,matrix,img.size()); imshow("rotateimg",outputImg);cpp_pcl_main_CloudViewer3d点云main
#include <iostream> #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/cloud_viewer.h> int main(int argc, char **argv) { // 创建PointCloud的智能指针 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>); // 加载pcd文件到cloud pcl::io::loadPCDFile("./data/pcl_logo.pcd", *cloud); pcl::visualization::CloudViewer viewer("Cloud Viewer"); //这里会一直阻塞直到点云被渲染 viewer.showCloud(cloud); // 循环判断是否退出 while (!viewer.wasStopped()) { // 你可以在这里对点云做很多处理 } return 0; }cpp_main纯
#include <iostream> int main(int argc, char *argv[]) { return 0; }cpp_pcl_CloudViewer点云设置背景色添加文字，形状
#include <pcl/visualization/cloud_viewer.h> #include <iostream> #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> int user_data; void viewerOneOff(pcl::visualization::PCLVisualizer &viewer) { // 设置背景色为粉红色 viewer.setBackgroundColor(1.0, 0.5, 1.0); pcl::PointXYZ o; o.x = 1.0; o.y = 0; o.z = 0; // 添加一个圆心为o，半径为0.25m的球体 最后以为 参数1的时候不显示图片 viewer.addSphere(o, 0.25, "sphere", 0); std::cout << "i only run once" << std::endl; } void viewerPsycho(pcl::visualization::PCLVisualizer &viewer) { static unsigned count = 0; std::stringstream ss; ss << "Once per viewer loop: " << count++; // 每次刷新时，移除text，添加新的text viewer.removeShape("text", 0); viewer.addText(ss.str(), 200, 300, "text", 0); //FIXME: possible race condition here: user_data++; } int main() { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::io::loadPCDFile("./data/pcl_logo.pcd", *cloud); pcl::visualization::CloudViewer viewer("Cloud Viewer"); //这里会一直阻塞直到点云被渲染 viewer.showCloud(cloud); // 只会调用一次 (非必须) viewer.runOnVisualizationThreadOnce (viewerOneOff); // 每次可视化迭代都会调用一次（频繁调用） (非必须) viewer.runOnVisualizationThread (viewerPsycho); while (!viewer.wasStopped()) { user_data++; } return 0; }cpp_pcl_PCLVisualizer_viewer点云点云设置个纯色添加一个坐标系_main
#include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/pcl_visualizer.h> int main(int argc, char **argv) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("./data/bunny.pcd", *cloud); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_milk(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::io::loadPCDFile("./data/milk_color.pcd", *cloud_milk); // 创建PCLVisualizer boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); // 设置背景色为灰色（非必须） viewer->setBackgroundColor (0.05, 0.05, 0.05, 0); // 添加一个普通点云 (可以设置指定颜色，也可以去掉single_color参数不设置) pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); viewer->addPointCloud<pcl::PointXYZ> (cloud, single_color, "sample cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud"); // 再添加一个彩色点云及配置 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_milk); viewer->addPointCloud<pcl::PointXYZRGB> (cloud_milk, rgb, "sample cloud milk"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud milk"); // 添加一个0.5倍缩放的坐标系（非必须） viewer->addCoordinateSystem (0.5); // 直到窗口关闭才结束循环 while (!viewer->wasStopped()) { // 每次循环调用内部的重绘函数 viewer->spinOnce(); } return 0; }cpp_pcl_PCLVisualizer_viewer点云点云设置个纯色添加一个坐标系
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D_Viewer")); viewer->setBackgroundColor(0.05, 0.05, 0.05, 0); // 给点云设置个纯色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 255, 0); viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample_cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample_cloud"); // 添加一个坐标系 viewer->addCoordinateSystem(0.5); while (!viewer->wasStopped()) { // 每次循环手动调用渲染函数 viewer->spinOnce(); }cpp_pcl_矩阵变换旋转平移_main
#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/io/ply_io.h> #include <pcl/point_cloud.h> #include <pcl/console/parse.h> #include <pcl/common/transforms.h> #include <pcl/visualization/pcl_visualizer.h> /** * 旋转并平移 * @param argc * @param argv * @return */ int main(int argc, char **argv) { // Load file | Works with PCD and PLY files pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>()); if (pcl::io::loadPCDFile(argv[1], *source_cloud) < 0) { std::cout << "Error loading point cloud " << argv[1] << std::endl << std::endl; return -1; } /* Reminder: how transformation matrices work : |-------> This column is the translation | 1 0 0 x | \ | 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left | 0 0 1 z | / | 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1) METHOD #1: Using a Matrix4f This is the "manual" method, perfect to understand but error prone ! */ Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) float theta = M_PI / 4; // The angle of rotation in radians transform_1(0, 0) = cos(theta); transform_1(0, 1) = -sin(theta); transform_1(1, 0) = sin(theta); transform_1(1, 1) = cos(theta); // (row, column) // Define a translation of 2.5 meters on the x axis. transform_1(0, 3) = 2.5; // Print the transformation printf("Method #1: using a Matrix4f\n"); std::cout << transform_1 << std::endl; /* METHOD #2: Using a Affine3f This method is easier and less error prone */ Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); // Define a translation of 0.8 meters on the x axis. transform_2.translation() << 0.8, 0.0, 0.0; // The same rotation matrix as before; theta radians arround Z axis transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); // Print the transformation printf("\nMethod #2: using an Affine3f\n"); std::cout << transform_2.matrix() << std::endl; // Executing the transformation pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>()); // You can either apply transform_1 or transform_2; they are the same pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2); // Visualization printf("\nPoint cloud colors : white = original point cloud\n" " red = transformed point cloud\n"); pcl::visualization::PCLVisualizer viewer("Matrix transformation example"); // Define R,G,B colors for the point cloud pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255); // We add the point cloud to the viewer and pass the color handler viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); // 设置坐标系系统 viewer.addCoordinateSystem(0.5, "cloud", 0); // 设置背景色 viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey // 设置渲染属性（点大小） viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); // 设置渲染属性（点大小） viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce(); } return 0; }cpp_pcl_矩阵变换旋转平移2种方法
/* Reminder: how transformation matrices work : |-------> This column is the translation | 1 0 0 x | \ | 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left | 0 0 1 z | / | 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1) METHOD #1: Using a Matrix4f This is the "manual" method, perfect to understand but error prone ! */ Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // 绕z轴顺时针旋转45° float theta = - M_PI / 4; transform_1(0, 0) = cos(theta); transform_1(0, 1) =-sin(theta); transform_1(1, 0) = sin(theta); transform_1(1, 1) = cos(theta); // 在x轴方向上平移1.5m transform_1(0, 3) = 1.5; std::cout << "transform_1:\n" << transform_1 << std::endl; // 方式二：使用Eigen提供的仿射变换对象 --------------------------------------------------- Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); // 执行旋转操作 z逆时针30度 transform_2.rotate(Eigen::AngleAxisf(DEG2RAD(30), Eigen::Vector3f::UnitZ())); // 执行x平移操作 平移1m transform_2.translation() << 1.0, 0, 0; std::cout << "transform_2:\n" << transform_2.matrix() << std::endl; // 执行变换并将之输出到transformed_cloud中 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2); // pcl::transformPointCloud(*cloud, *transformed_cloud, transform_1); //显示 viewer->addPointCloud<pcl::PointXYZ>(transformed_cloud, "sample_cloud1"); //viewer->addPointCloud<pcl::PointXYZ>(transformed_cloud, "sample_cloud2"); // 添加一个坐标系 viewer->addCoordinateSystem(0.5); while (!viewer->wasStopped()) { // 每次循环手动调用渲染函数 viewer->spinOnce(); }cpp_pcl_反序列化_main
#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> int main(int argc, char **argv) { // 准备pcl::PointXYZ类型的点云 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 将pcd中的数据加载到cloud中 if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/bunny.pcd", *cloud) == -1) //* load the file { PCL_ERROR ("Couldn't read file bunny.pcd \n"); return (-1); } std::cout << "Loaded " << cloud->width * cloud->height << " data points from test_pcd.pcd with the following fields: " << std::endl; for (size_t i = 0; i < cloud->points.size(); ++i) std::cout << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl; return (0); }cpp_pcl_序列化_main
#include <iostream> #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> using namespace std; /** * 点云的序列化 */ int main(int argc, char *argv[]) { pcl::PointCloud<pcl::PointXYZ> cloud; cloud.width = 5; cloud.height = 1; cloud.is_dense = false; uint32_t points_count = cloud.width * cloud.height; cloud.points.resize(points_count); // 生成一些随机数， 写到文件 for (int i = 0; i < points_count; ++i) { // (0 -> 1.0) * 1024.0f >> 0 -> 1024.0f cloud.points[i].x = 1024.0f * (rand() / (RAND_MAX + 1.0f)); cloud.points[i].y = 1024.0f * (rand() / (RAND_MAX + 1.0f)); cloud.points[i].z = 1024.0f * (rand() / (RAND_MAX + 1.0f)); std::cout << "x: " << cloud.points[i].x << " y: " << cloud.points[i].y << " z: " << cloud.points[i].z << std::endl; } pcl::io::savePCDFileASCII("./output/pcd_ascii.pcd", cloud); pcl::io::savePCDFileBinary("./output/pcd_binary.pcd", cloud); pcl::io::savePCDFileBinaryCompressed("./output/pcd_binary_compressed.pcd", cloud); pcl::io::savePCDFile("./output/pcd_save.pcd", cloud, true); // savePCDFileBinary }cpp_pcl_k-dtree-两种方法main
#include <pcl/point_cloud.h> #include <pcl/kdtree/kdtree_flann.h> #include <iostream> #include <vector> #include <ctime> //#include <pcl/search/kdtree.h> //#include <pcl/search/impl/search.hpp> #include <pcl/visualization/cloud_viewer.h> int main(int argc, char **argv) { // 用系统时间初始化随机种子 srand(time(NULL)); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 生成点云数据1000个 cloud->width = 1000; cloud->height = 1; // 1 表示点云为无序点云 cloud->points.resize(cloud->width * cloud->height); // 给点云填充数据 0 - 1023 for (size_t i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f); } // 创建KdTree的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor) pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::search::KdTree<pcl::PointXYZ> kdtree; // 设置搜索空间，把cloud作为输入 kdtree.setInputCloud(cloud); // 初始化一个随机的点，作为查询点 pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // K nearest neighbor search // 方式一：搜索K个最近邻居 // 创建K和两个向量来保存搜索到的数据 // K = 10 表示搜索10个临近点 // pointIdxNKNSearch 保存搜索到的临近点的索引 // pointNKNSquaredDistance 保存对应临近点的距离的平方 int K = 10; std::vector<int> pointIdxNKNSearch(K); std::vector<float> pointNKNSquaredDistance(K); std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl; } // Neighbors within radius search // 方式二：通过指定半径搜索 std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; // 创建一个随机[0,256)的半径值 float radius = 256.0f * rand() / (RAND_MAX + 1.0f); std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个以搜索点为圆心，搜索半径为半径的球体 viewer.addSphere(searchPoint, radius, "sphere", 0); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); } return 0; }cpp_pcl_k-dtree_nearestKSearch邻近法搜索
// 创建KdTree的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor) pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::search::KdTree<pcl::PointXYZ> kdtree; // 设置搜索空间，把cloud作为输入 kdtree.setInputCloud(cloud); // 初始化一个随机的点，作为查询点 pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // K nearest neighbor search // 方式一：搜索K个最近邻居 // 创建K和两个向量来保存搜索到的数据 // K = 10 表示搜索10个临近点 // pointIdxNKNSearch 保存搜索到的临近点的索引 // pointNKNSquaredDistance 保存对应临近点的距离的平方 int K = 10; std::vector<int> pointIdxNKNSearch(K); std::vector<float> pointNKNSquaredDistance(K); std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; // 执行knn -> k邻域搜索， 返回值邻居的个数 if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl; } // 显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_k-dtree_radiusSearch半径搜索
// Neighbors within radius search // 方式二：通过指定半径搜索 std::vector<int> pointIdxRadiusSearch; // 准备好输出结果列表 - 邻居距离平方的列表 std::vector<float> pointRadiusSquaredDistance; // 创建一个随机[0,256)的半径值 float radius = 256.0f * rand() / (RAND_MAX + 1.0f); std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } //显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加一个以搜索点为圆心，搜索半径为半径的球体 viewer.addSphere(searchPoint, radius, "sphere", 0); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_octree八叉树搜索_main
#include <pcl/point_cloud.h> #include <pcl/octree/octree_search.h> #include <iostream> #include <vector> #include <ctime> #include <pcl/visualization/cloud_viewer.h> int main(int argc, char **argv) { srand((unsigned int) time(NULL)); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // Generate pointcloud data cloud->width = 1000; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f); } // float resolution = 0.01f; // 设置分辨率为128 float resolution = 128.0f; // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置输入点云 octree.setInputCloud(cloud); // 通过点云构建octree octree.addPointsFromInputCloud(); pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // Neighbors within voxel search // 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回, // 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数 std::vector<int> pointIdxVec; if (octree.voxelSearch(searchPoint, pointIdxVec)) { std::cout << "Neighbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl; for (size_t i = 0; i < pointIdxVec.size(); ++i) std::cout << " " << cloud->points[pointIdxVec[i]].x << " " << cloud->points[pointIdxVec[i]].y << " " << cloud->points[pointIdxVec[i]].z << std::endl; } // K nearest neighbor search // 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中, // 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉 // 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。 int K = 10; std::vector<int> pointIdxNKNSearch; std::vector<float> pointNKNSquaredDistance; std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl; } // Neighbors within radius search // 方式三：半径内近邻搜索 // “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中, // 这两个向量分别存储结果点的索引和对应的距离平方 std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; float radius = 256.0f * rand() / (RAND_MAX + 1.0f); std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } //显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个以搜索点为圆心，搜索半径为半径的球体 viewer.addSphere(searchPoint, radius, "sphere", 0); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); } }cpp_pcl_随机创建点云图_main
#include <pcl/point_cloud.h> #include <pcl/octree/octree_search.h> #include <iostream> #include <vector> #include <ctime> #include <pcl/visualization/cloud_viewer.h> int main(int argc, char **argv) { srand((unsigned int) time(NULL)); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // Generate pointcloud data cloud->width = 1000; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f); } //显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); } }cpp_srand随机种子
#include <ctime> //放main方法下面 srand((unsigned int) time(NULL));cpp_rand取随机值
1024.0f * rand() / (RAND_MAX + 1.0f)cpp_pcl_octree八叉树搜索_main
#include <pcl/point_cloud.h> #include <pcl/octree/octree_search.h> #include <iostream> #include <vector> #include <ctime> #include <pcl/visualization/cloud_viewer.h> int main(int argc, char **argv) { srand((unsigned int) time(NULL)); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // Generate pointcloud data cloud->width = 1000; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f); cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f); } // float resolution = 0.01f; // 设置分辨率为128 float resolution = 128.0f; // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置输入点云 octree.setInputCloud(cloud); // 通过点云构建octree octree.addPointsFromInputCloud(); pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // Neighbors within voxel search // 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回, // 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数 std::vector<int> pointIdxVec; if (octree.voxelSearch(searchPoint, pointIdxVec)) { std::cout << "Neighbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl; for (size_t i = 0; i < pointIdxVec.size(); ++i) std::cout << " " << cloud->points[pointIdxVec[i]].x << " " << cloud->points[pointIdxVec[i]].y << " " << cloud->points[pointIdxVec[i]].z << std::endl; } // K nearest neighbor search // 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中, // 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉 // 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。 int K = 10; std::vector<int> pointIdxNKNSearch; std::vector<float> pointNKNSquaredDistance; std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl; } // Neighbors within radius search // 方式三：半径内近邻搜索 // “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中, // 这两个向量分别存储结果点的索引和对应的距离平方 std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; float radius = 256.0f * rand() / (RAND_MAX + 1.0f); std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个以搜索点为圆心，搜索半径为半径的球体 viewer.addSphere(searchPoint, radius, "sphere", 0); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); } }cpp_pcl_octree八叉树搜索_voxelSearch体素近邻搜索
// float resolution = 0.01f; // 设置分辨率为128 float resolution = 128.0f; // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置输入点云 octree.setInputCloud(cloud); // 通过点云构建octree octree.addPointsFromInputCloud(); pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // Neighbors within voxel search // 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回, // 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数 std::vector<int> pointIdxVec; if (octree.voxelSearch(searchPoint, pointIdxVec)) { std::cout << "Neighbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl; for (size_t i = 0; i < pointIdxVec.size(); ++i) std::cout << " " << cloud->points[pointIdxVec[i]].x << " " << cloud->points[pointIdxVec[i]].y << " " << cloud->points[pointIdxVec[i]].z << std::endl; } //显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_octree八叉树搜索_nearestKSearch近邻搜索
// float resolution = 0.01f; // 设置分辨率为128 float resolution = 128.0f; // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置输入点云 octree.setInputCloud(cloud); // 通过点云构建octree octree.addPointsFromInputCloud(); pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // K nearest neighbor search // 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中, // 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉 // 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。 int K = 10; std::vector<int> pointIdxNKNSearch; std::vector<float> pointNKNSquaredDistance; std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl; } //显示 pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_octree八叉树搜索_radiusSearch半径内近邻搜索
// float resolution = 0.01f; // 设置分辨率为128 float resolution = 128.0f; // resolution该参数描述了octree叶子leaf节点的最小体素尺寸。 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置输入点云 octree.setInputCloud(cloud); // 通过点云构建octree octree.addPointsFromInputCloud(); pcl::PointXYZ searchPoint; searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f); searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f); // Neighbors within radius search // 方式三：半径内近邻搜索 // “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中, // 这两个向量分别存储结果点的索引和对应的距离平方 std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; float radius = 256.0f * rand() / (RAND_MAX + 1.0f); std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl; if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl; } pcl::visualization::PCLVisualizer viewer("PCL Viewer"); viewer.setBackgroundColor(0.0, 0.0, 0.5); viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); pcl::PointXYZ originPoint(0.0, 0.0, 0.0); // 添加从原点到搜索点的线段 viewer.addLine(originPoint, searchPoint); // 添加一个放到200倍后的坐标系 viewer.addCoordinateSystem(200); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_CloudViewer显示
pcl::visualization::CloudViewer viewer("clound viewer"); //此代码会阻塞指导渲染完毕 viewer.showCloud(cloud); //死循环判断窗口是否退出 用户按Q while (!viewer.wasStopped()){ //不做任何处理,可以在这里更新点云内容 }cpp_pcl_CloudViewer显示_给点云设置个背景色
#include <iostream> #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/cloud_viewer.h> void viewerOnce(pcl::visualization::PCLVisualizer &visualizer){ // 给点云设置个背景色 visualizer.setBackgroundColor(1.0, 0.5, 1.0); pcl::PointXYZ center; center.x = 1.0; center.y = 1.0; center.z = 0; visualizer.addSphere(center, 0.25, "sphere", 0); std::cout << "viewer Once" << std::endl; }; int counter = 0; int user_counter = 0; void viewerPsycho(pcl::visualization::PCLVisualizer &visualizer){ // 先将之前添加的移除掉 visualizer.removeShape("text_str", 0); stringstream ss; ss << "Per viewer loop: " << counter++; visualizer.addText(ss.str(), 100, 300, 22, 1.0,1.0,1.0, "text_str", 0); std::cout << "viewer Psycho: c->" << counter << " uc->" << user_counter << std::endl; }; // 可视化展示 pcl::visualization::CloudViewer viewer("cloudViewer"); viewer.showCloud(cloud); // 1. 在首次渲染时候执行操作（添加球体） viewer.runOnVisualizationThreadOnce(viewerOnce); // 2. 在每次更新渲染时候执行操作（添加文字） viewer.runOnVisualizationThread(viewerPsycho); while (!viewer.wasStopped()) { // 可以在此处更新点云 user_counter++; }cpp_pcl_PCLVisualizer_viewer显示
#include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/pcl_visualizer.h> pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D_Viewer")); viewer->setBackgroundColor(0.05, 0.05, 0.05, 0); // 给点云设置个纯色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 255, 0); viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample_cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample_cloud"); viewer->addPointCloud<pcl::PointXYZRGBA>(cloud_milk, "sample_color_cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample_color_cloud"); // 添加一个坐标系 viewer->addCoordinateSystem(0.5); while (!viewer->wasStopped()) { // 每次循环手动调用渲染函数 viewer->spinOnce(); }cpp_opencv_threshold二值化
threshold(src,src1,0,255,THRESH_BINARY|THRESH_OTSU);//自动OTSU阈值threshold(tg, t2, 0, 255, THRESH_OTSU|tyv);cpp_opencv_inpaint图片修复_main
#include <opencv2/opencv.hpp> #include <iostream> using namespace cv; using namespace std; /** * * @return */ Mat src = imread("../img/itheima_inpaint.jpg"); Mat inpaintMask = Mat::zeros(src.rows,src.cols,CV_8UC1); void onMouse(int event, int x, int y, int flags, void* userdata){ // 当用户按着左键进行滑动的时候。 绘制修复路径 if(event == EVENT_MOUSEMOVE && (flags == EVENT_FLAG_LBUTTON)){ cout<<"按着左键滑动：x="<<x<<" y="<<y<<endl; circle(inpaintMask,Point(x,y),5,Scalar(255),-1); imshow("inpaintMask",inpaintMask); }else if(event == EVENT_LBUTTONUP){ cout<<"左键抬起"<<endl; double radius = 5; Mat dst; inpaint(src,inpaintMask,src,radius,INPAINT_TELEA); imshow("src",src); } } int main(){ imshow("src",src); setMouseCallback("src",onMouse); waitKey(); return 0; }cpp_pcl_PassThrough直通滤波
#include <pcl/filters/passthrough.h> pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filterd(new pcl::PointCloud<pcl::PointXYZ>);//存放过滤后的点云 // 创建滤波器对象 pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud(cloud); //加载原点云 pass.setFilterFieldName("z"); // 要过滤的字段 pass.setFilterLimits(0.0, 1.0); // 设置保留范围 // pass.setFilterLimitsNegative(true); // 默认false，如果true， 保留Limits范围之外内容 pass.filter(*cloud_filterd); // 执行过滤，并把结果放到cloud_filterd std::cout << "过滤后留下的点------------------------" << std::endl; for (int i = 0; i < cloud_filterd->points.size(); ++i) { //打印 printf("(%f, %f, %f)\n", cloud_filterd->points[i].x, cloud_filterd->points[i].y, cloud_filterd->points[i].z); } pcl::visualization::CloudViewer viewer("cloud_viewer"); viewer.showCloud(cloud_filterd); while (!viewer.wasStopped()) { }cpp_pcl_VoxelGrid 降采样
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2); pcl::PCLPointCloud2::Ptr cloud_filterd(new pcl::PCLPointCloud2); // pcl::io::loadPCDFile("./data/table_scene_lms400.pcd", *cloud); pcl::PCDReader reader; reader.read("./data/table_scene_lms400.pcd", *cloud); std::cout << "过滤前的点个数：" << cloud->width * cloud->height << std::endl; std::cout << "数据类型：" << pcl::getFieldsList(*cloud)<< std::endl; float leafSize = 0.01f; // 执行降采样------------------------ pcl::VoxelGrid<pcl::PCLPointCloud2> voxel; // 设置输入点云 voxel.setInputCloud(cloud); // 设置体素网格的大小 voxel.setLeafSize(leafSize, leafSize, leafSize); // 执行滤波, 并将结果保存到cloud_filterd voxel.filter(*cloud_filterd); std::cout << "---------------------------------------------" << std::endl; std::cout << "过滤后的点个数：" << cloud_filterd->width * cloud_filterd->height << std::endl; std::cout << "数据类型：" << pcl::getFieldsList(*cloud_filterd)<< std::endl; pcl::PCDWriter writer; writer.write("./data/table_scene_lms400_downsample.pcd", *cloud_filterd);cpp_pcl_StatisticalOutlierRemoval离群点移除过滤器
#include <iostream> #include <pcl/point_cloud.h> #include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/statistical_outlier_removal.h> typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; using namespace std; /** * */ int main(int argc, char *argv[]) { PointCloud::Ptr cloud(new PointCloud); PointCloud::Ptr cloud_filtered(new PointCloud); pcl::PCDReader reader; reader.read<PointT>("./data/table_scene_lms400_downsample.pcd", *cloud); // 创建统计学离群点移除过滤器 pcl::StatisticalOutlierRemoval<PointT> sor; sor.setInputCloud(cloud); // 设置过滤邻域K sor.setMeanK(50); // 设置标准差的系数, 值越大，丢掉的点越少 sor.setStddevMulThresh(1.0f); sor.filter(*cloud_filtered); // 保存去掉离群点之后的点云 pcl::PCDWriter writer; writer.write("./data/table_scene_lms400_inliers.pcd", *cloud_filtered); // 取反 sor.setNegative(true); sor.filter(*cloud_filtered); // 保存被去掉离群点 writer.write("./data/table_scene_lms400_outliers.pcd", *cloud_filtered); }cpp_pcl_ConditionalRemoval条件滤波
#include <pcl/filters/radius_outlier_removal.h> #include <pcl/filters/conditional_removal.h> pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); // 条件滤波 // 创建条件对象 0.0 < z <= 0.8 || x > 0 pcl::ConditionAnd<PointType>::Ptr range_cond(new pcl::ConditionAnd<PointType>); // 添加比较对象 z > 0.0 range_cond->addComparison(pcl::FieldComparison<PointType>::ConstPtr(new pcl::FieldComparison<PointType>( "z", pcl::ComparisonOps::GT, 0.0))); // 添加比较对象 z <= 0.8 range_cond->addComparison(pcl::FieldComparison<PointType>::ConstPtr(new pcl::FieldComparison<PointType>( "z", pcl::ComparisonOps::LE, 0.8))); pcl::ConditionalRemoval<PointType> condRem; condRem.setInputCloud(cloud); // 设置过滤条件 condRem.setCondition(range_cond); // false默认值：将条件之外的点移除掉 // true：将条件之外的点还保留，但是设置NAN, 或者setUserFilterValue修改 condRem.setKeepOrganized(false); condRem.filter(*cloud_filtered); pcl::visualization::PointCloudColorHandlerCustom<PointType> cloud_filtered_color(cloud, 255, 0, 0); viewer.addPointCloud(cloud_filtered, cloud_filtered_color, "cloud_filtered"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "cloud_filtered"); viewer.addCoordinateSystem(1.0); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_pcl_RadiusOutlierRemoval半径滤波
#include <pcl/filters/radius_outlier_removal.h> #include <pcl/filters/conditional_removal.h> pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); // 通过半径滤波执行, 如果一个点在指定半径内，有指定个数的邻居，则保留 pcl::RadiusOutlierRemoval<PointType> outlierRemoval; outlierRemoval.setInputCloud(cloud); // 设置搜索半径 outlierRemoval.setRadiusSearch(0.4); // 邻居个数 outlierRemoval.setMinNeighborsInRadius(2); outlierRemoval.filter(*cloud_filtered); pcl::visualization::PointCloudColorHandlerCustom<PointType> cloud_filtered_color(cloud, 255, 0, 0); viewer.addPointCloud(cloud_filtered, cloud_filtered_color, "cloud_filtered"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "cloud_filtered"); viewer.addCoordinateSystem(1.0); while (!viewer.wasStopped()) { viewer.spinOnce(); }cpp_h弧度角度互转
float DE2RA = M_PI / 180.0f; float RA2DE = 180.0f / M_PI; // angular_resolution： 扫描的分辨率，1° -> 弧度。 float angular_resolution = DEG2RAD(1.0f); // degrees to radians // max_angle_width 水平方向扫描的范围 360° -> 弧度 float max_angle_width = 360.0f * DE2RA; // max_angle_height 竖直方向扫描范围 180° -> 弧度 float max_angle_height = 180.0f * DE2RA;cpp_pcl_通过点云生成深度图_main
#include <iostream> #include <pcl/io/io.h> #include <pcl/range_image/range_image.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/io/pcd_io.h> using namespace std; /** * 通过点云生成深度图 */ int main(int argc, char *argv[]) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ> &pointCloud = *cloud; // 添加一个正方形区域 -0.5 -> 0.5 for (float y = -0.5f; y <= 0.5f; y += 0.01f) { for (float z = -0.5f; z <= 0.5f; z += 0.01f) { pcl::PointXYZ point; point.x = 2.0f - y; point.y = y; point.z = z; pointCloud.points.push_back(point); } } uint32_t size = pointCloud.points.size(); pointCloud.width = size; pointCloud.height = 1; // pcl::io::loadPCDFile("./data/table_scene_lms400_downsample.pcd", pointCloud); float DE2RA = M_PI / 180.0f; float RA2DE = 180.0f / M_PI; // * \param angular_resolution the angular difference (in radians) between the individual pixels in the image // angular_resolution： 扫描的分辨率，1° -> 弧度。 float angular_resolution = DEG2RAD(1.0f); // degrees to radians // * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor // max_angle_width 水平方向扫描的范围 360° -> 弧度 float max_angle_width = 360.0f * DE2RA; // * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor // max_angle_height 竖直方向扫描范围 180° -> 弧度 float max_angle_height = 180.0f * DE2RA; // * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to // * Eigen::Affine3f::Identity () ) Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, -3.0f, 0.0f); // * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME) pcl::RangeImage::CoordinateFrame coordinateFrame = pcl::RangeImage::CAMERA_FRAME; // * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum, // * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and will always take the minimum per cell. float noise_level = 0.0f; float minRange = 0.0f; // * \param border_size the border size (defaults to 0) float border_size = 0.0f; // 从点云图生成深度图 pcl::RangeImage::Ptr rangeImagePtr(new pcl::RangeImage); pcl::RangeImage &rangeImage = *rangeImagePtr; rangeImage.createFromPointCloud(pointCloud, angular_resolution, max_angle_width, max_angle_height, sensorPose, coordinateFrame, noise_level, minRange, border_size ); pcl::visualization::PCLVisualizer viewer("3dViewer"); viewer.setBackgroundColor(1.0, 1.0, 1.0); // 添加的原始点云 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 100, 0); viewer.addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample_cloud"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample_cloud"); // 添加深度图 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_color(rangeImagePtr, 0, 0, 0); viewer.addPointCloud(rangeImagePtr, range_color, "range_image"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "range_image"); viewer.addCoordinateSystem(1.0); while (!viewer.wasStopped()) { viewer.spinOnce(); pcl_sleep(0.01); } }cpp_pcl_SampleConsensusModelPlane_SampleConsensusModelSphere查找创建包含外部点的平面及球体_main
#include <iostream> #include <thread> #include <pcl/console/parse.h> #include <pcl/filters/extract_indices.h> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/sample_consensus/ransac.h> #include <pcl/sample_consensus/sac_model_plane.h> #include <pcl/sample_consensus/sac_model_sphere.h> #include <pcl/visualization/pcl_visualizer.h> using namespace std; /** * 使用方法： * * random_sample_consensus 创建包含外部点的平面 * random_sample_consensus -f 创建包含外部点的平面，并计算平面内部点 * * random_sample_consensus -s 创建包含外部点的球体 * random_sample_consensus -sf 创建包含外部点的球体，并计算球体内部点 */ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr inliers_cloud(new pcl::PointCloud<pcl::PointXYZ>); // 根据参数生成点云数据 cloud->width = 500; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); bool isSphere = pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0; for (int i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); if (isSphere) { // -s 生成包含外部点球体 if (i % 5 == 0) { cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); } else if (i % 2 == 0) { cloud->points[i].z = sqrt(1 - (pow(cloud->points[i].x, 2) + pow(cloud->points[i].y, 2))); } else { cloud->points[i].z = -sqrt(1 - (pow(cloud->points[i].x, 2) + pow(cloud->points[i].y, 2))); } }else { // 生成包含外部点平面 -1.0 -> 1.0 if (i % 2 == 0) { cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); }else { cloud->points[i].z = -1.0f * (cloud->points[i].x + cloud->points[i].y); } } } bool isRansac = pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0; if (isRansac) { pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p( new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud)); pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s( new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud)); // 用于接收所有内部点的索引 std::vector<int> inliers; // 根据模型迭代拟合出平面模型参数，并获取符合其模型参数的内部点 pcl::SampleConsensusModel<pcl::PointXYZ>::Ptr model_arg = model_p; if (isSphere) { // 根据模型迭代拟合出球体模型参数，并获取符合其模型参数的内部点 model_arg = model_s; } pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_arg); // 阈值 1cm ransac.setDistanceThreshold(0.01f); // 执行计算 ransac.computeModel(); // 获取内点 ransac.getInliers(inliers); // 根据索引列表拷贝点云 pcl::copyPointCloud(*cloud, inliers, *inliers_cloud); }cpp_pcl_生成包含外部点球体
for (int i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); // -s 生成包含外部点球体 圆的算法 1=z平方 + x 平方 + y平方; z平方=z1 - (x 平方 + y平方) if (i % 5 == 0) { cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); } else if (i % 2 == 0) { cloud->points[i].z = sqrt(1 - (pow(cloud->points[i].x, 2) + pow(cloud->points[i].y, 2))); } else { cloud->points[i].z = -sqrt(1 - (pow(cloud->points[i].x, 2) + pow(cloud->points[i].y, 2))); } } }cpp_pcl_生成包含外部点平面
for (int i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); // 生成包含外部点平面 -1.0 -> 1.0 if (i % 2 == 0) { cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); }else { cloud->points[i].z = -1.0f * (cloud->points[i].x + cloud->points[i].y); } }cpp_pcl_ConditionAnd_or创建比较起0.0 < z <= 0.8 || x > 0
// 条件滤波 // 创建条件对象 0.0 < z <= 0.8 || x > 0 pcl::ConditionAnd<PointType>::Ptr range_cond(new pcl::ConditionAnd<PointType>); // 添加比较对象 z > 0.0 range_cond->addComparison(pcl::FieldComparison<PointType>::ConstPtr(new pcl::FieldComparison<PointType>( "z", pcl::ComparisonOps::GT, 0.0))); // 添加比较对象 z <= 0.8 range_cond->addComparison(pcl::FieldComparison<PointType>::ConstPtr(new pcl::FieldComparison<PointType>( "z", pcl::ComparisonOps::LE, 0.8)));cpp_pcl_NormalEstimation计算点云法向量_main
#include <iostream> #include <pcl/io/io.h> #include <pcl/features/normal_3d.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/cloud_viewer.h> #include <thread> using namespace std; /** * 评估点云法向量 * * NormalEstimation * * setInputCloud * * setIndices * * setSearchSurface */ int main(int argc, char *argv[]) { char *arg = argv[1]; if (!argv[1]) { arg = "./data/bunny.pcd"; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile(arg, *cloud); // 创建法线集合 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 计算点云法向量 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation; // 设置输入点云 normalEstimation.setInputCloud(cloud); // 设置搜索半径, 对于每个点，通过其周围半径3cm以内的邻居计算法线 normalEstimation.setRadiusSearch(0.03); // normalEstimation.setIndices() // normalEstimation.setSearchSurface() pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>()); // 设置搜索方法 normalEstimation.setSearchMethod(kdtree); // 估算法线 normalEstimation.compute(*normals); pcl::visualization::PCLVisualizer viewer("PCLViewer"); viewer.addPointCloud(cloud, "cloud"); // 每几个点绘制一个法向量 int level = 1; float scale = 0.01; viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, level, scale, "normal_cloud"); viewer.addCoordinateSystem(0.01); while (!viewer.wasStopped()) { viewer.spinOnce(); std::this_thread::sleep_for(100ms); } return 0; }cpp_pcl_NormalEstimation子集中进行表面法线估算_main
#include <iostream> #include <pcl/io/io.h> #include <pcl/features/normal_3d.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/cloud_viewer.h> #include <thread> using namespace std; /** * 评估点云法向量 * * NormalEstimation * * setInputCloud * * setIndices * * setSearchSurface */ int main(int argc, char *argv[]) { char *arg = argv[1]; if (!argv[1]) { arg = "./data/bunny.pcd"; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile(arg, *cloud); // 创建法线集合 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 计算点云法向量 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation; // 设置输入点云 normalEstimation.setInputCloud(cloud);cpp_pcl_NormalEstimation降采样版本法线估算_main
// // Created by sk on 2020/8/5. // #include <iostream> #include <pcl/io/io.h> #include <pcl/features/normal_3d.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/voxel_grid.h> #include <pcl/visualization/cloud_viewer.h> #include <thread> using namespace std; /** * 评估点云法向量 * * NormalEstimation * * setInputCloud * * setIndices * * setSearchSurface */ int main(int argc, char *argv[]) { char *arg = argv[1]; if (!argv[1]) { arg = "./data/bunny.pcd"; } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile(arg, *cloud); // 创建法线集合 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 降采样原始点云 pcl::VoxelGrid<pcl::PointXYZ> voxel; voxel.setInputCloud(cloud); voxel.setLeafSize(0.03f, 0.03f, 0.03f ); voxel.filter(*cloud_downsampled); // 计算点云法向量 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation; // 设置输入点云 normalEstimation.setInputCloud(cloud_downsampled); // 设置搜索半径, 对于每个点，通过其周围半径3cm以内的邻居计算法线 normalEstimation.setRadiusSearch(0.03); // normalEstimation.setViewPoint() // normalEstimation.setIndices(indicesPtr); normalEstimation.setSearchSurface(cloud); pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>()); // 设置搜索方法 normalEstimation.setSearchMethod(kdtree); // 估算法线 normalEstimation.compute(*normals); pcl::visualization::PCLVisualizer viewer("PCLViewer"); viewer.addPointCloud(cloud, "cloud"); // 这里的点云数量必须和法线数量一致 ------------------------------------ ! // 每几个点绘制一个法向量 int level = 1; float scale = 0.02; viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_downsampled, normals, level, scale, "normal_cloud"); // 所有原始点 viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); // 降采样后的点 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); viewer.addPointCloud<pcl::PointXYZ>(cloud_downsampled, single_color, "cloud_downsampled"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud_downsampled"); viewer.addCoordinateSystem(0.01); while (!viewer.wasStopped()) { viewer.spinOnce(); std::this_thread::sleep_for(100ms); } return 0; }
