0403
0404
- using和typedef的区别:非模板工作时,两者相同;但是using可以适用于模板;详细链接
- C++的多种关键字
- static关键字:修饰全局变量或函数:仅本文件使用;修饰局部变量:值不变,作用域仅在定义语句块中;修饰类内成员
- const关键字:
- constexpr关键字:修饰变量编译器检查变量是否为const表达式;修饰函数用于仅有一条return的函数,形参和返回值都是字面值类型;
- volatile关键字:修饰类型表示变量可以被某些编译器未知因素修改,比如操作系统,硬件或者其他线程,常用于多个任务共享的变量定义;
0406
0413
- C++列表初始化:详解;效率更高一点,赋值顺序是按照变量的声明顺序进行的;
0414
0420
- 单例模式和工厂模式:详解;
0423
- 点云一致性分割:翻译详解
0425
- 平面拟合:
#include <Eigen/Dense>
Eigen::Vector4d fitPlane(const std::vector<Eigen::Vector3d>& points, int mode) {
int n = points.size();
Eigen::MatrixXd A(n, 4);
if(mode == 0){//todo: core SVD
for (int i = 0; i < n; ++i) {
A(i, 0) = points[i](0);
A(i, 1) = points[i](1);
A(i, 2) = points[i](2);
A(i, 3) = 1;
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU);
Eigen::VectorXd singularValues = svd.singularValues();
double epsilon = 1e-5;
for (int i = 0; i < singularValues.size(); ++i) {
if (singularValues(i) < epsilon) {
singularValues(i) = 0;
}
else {
singularValues(i) = 1 / singularValues(i);
}
}
Eigen::MatrixXd S = Eigen::MatrixXd::Zero(4, n);
S.topLeftCorner(3, 3) = svd.matrixV().transpose();
S.bottomRightCorner(1, 1) = singularValues.asDiagonal();
Eigen::Matrix4d pinv = S * svd.matrixU().transpose();
return pinv.col(3);
}
else if(mode == 1){//COV
Eigen::Vector3d centorid(0,0,0);
for(int i =0; i < n; ++i){
centorid(0) += points[i].x();
centorid(1) += points[i].y();
centorid(2) += points[i].z();
}
centorid /= n;
for (int i = 0; i < n; ++i) {
A(i, 0) = points[i](0) - centorid.x();
A(i, 1) = points[i](1) - centorid.y();
A(i, 2) = points[i](2) - centorid.z();
A(i, 3) = 1;
}
//caculate normal
Eigen::MatrixXd cov_mat = A.transpose() * A;
cov_mat = cov_mat.array() / A.rows();
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(cov_mat);
Eigen::MatrixXd vec = eigen_solver.eigenvectors();//
Eigen::MatrixXd val = eigen_solver.eigenvalues();
Eigen::Vector3d normal = vec.col(0);
// Eigen::Vector3d normal = vec.row(0);
Eigen::Vector4d abcd = Eigen::Vector4d::Zero();
double d = -(normal[0] * centorid.x() + normal[1] * centorid.y() + normal[2] * centorid.z());
abcd << normal[0], normal[1], normal[2], d;
return abcd;
}
else{
SM_ERROR_STREAM("no this mode!!!");
}
}
0505
- pcl点云去质心计算;
- interrupted by signal 8: SIGFPE:表示除0异常;
- pcl自带的去质心函数:LINK
- 关于ros中仿真模型的组织形式和语法:
- 如何利用宏定义减少重复代码
- 如何组织多个部件组成一个整体
- 摄像头仿真
- 搭建教程
- ChatGPT的回答
如果你希望在机器人模型的不同位置上放置多个 camera,可以使用 xacro 的宏定义功能来创建多个相似的链接和传感器。以下是一个可能的实现方法:
首先,定义一个名为camera_macro
的 xacro 宏,用于创建一个包含一个链接和一个相机的子系统。该宏可以接受四个参数:链接名称、链接姿态、相机名称和相机分辨率。例如:
<!-- camera_macro.xacro -->
<xacro:macro name="camera_macro" params="link_name link_pose camera_name resolution">
<link name="${link_name}">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<gazebo reference="${link_name}">
<sensor type="camera" name="${camera_name}">
<camera>
<image>
<width>${resolution}</width>
<height>${resolution}</height>
</image>
<clip>
<near>0.01</near>
<far>1000.0</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.001</stddev>
</noise>
<view_controller>orbit</view_controller>
</camera>
<always_on>true</always_on>
<update_rate>30.0</update_rate>
<camera>
<horizontal_fov>1.3962634</horizontal_fov>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>${camera_name}</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>${link_name}</frameName>
<hackBaseline>0.07</hackBaseline>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
然后,在机器人模型的 xacro 文件中调用 camera_macro
宏,并传递不同的参数来创建多个相似的链接和相机。例如,以下代码在机器人的底盘、头部和手臂上各放置了一个相机:
<!-- robot.xacro -->
...
<xacro:camera_macro link_name="base_camera" link_pose="0 0 0 0 0 0"
camera_name="base_camera_sensor" resolution="640"/>
<xacro:camera_macro link_name="head_camera" link_pose="0 0 0.1 0 0 0"
camera_name="head_camera_sensor" resolution="480"/>
<xacro:camera_macro link_name="arm_camera" link_pose="0.2 0 0.1 0 0 0"
camera_name="arm_camera_sensor" resolution="320"/>
...
在上述代码中,我们分别调用了 camera_macro
宏三次,分别创建了名为 base_camera
、head_camera
和 arm_camera
的链接和相机。这些相机分别放置在机器人的底盘、头部和手臂上,具有不同的名称和分辨率。
希望这个例子能够帮助你理解如何使用 xacro 创建多个相似的链接和传感器!
0508
- gazebo设置光源形式:LINK
0510
- 点云拟合平面:LINK
0511
0512
//
// Created by lixiang on 2023/5/12.
//
#include <csignal>
#include <ros/ros.h>
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include "sensor_msgs/PointCloud2.h"
#include "boost/thread/thread.hpp"
#include "logging/logging.hpp"
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2>;
void shutSigintHandler(int sig) {
SM_WARN_STREAM("ros node shutdown!");
ros::shutdown();
}
void calibCallback(sensor_msgs::PointCloud2ConstPtr lidar_detect,
sensor_msgs::PointCloud2ConstPtr camera_detect){ ··· }
int main(int argc, char** argv){
ros::init(argc, argv, "lidar_camera_calib");
ros::NodeHandle nh_("~"); // LOCAL
signal(SIGINT, shutSigintHandler);// CTRL+C for exit
std::string lidar_detect_topic, camera_detect_topic;
nh_.param<std::string>("lidar_detect_topic", lidar_detect_topic, "/lidar_pattern/centers_cloud");
nh_.param<std::string>("camera_detect_topic", camera_detect_topic, "/mono_pattern_0/centers_pts_cloud");
//订阅和同步语句不能放在if的执行内容中
message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh_, lidar_detect_topic, 20);
message_filters::Subscriber<sensor_msgs::PointCloud2> camera_sub(nh_, camera_detect_topic, 20);
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(20), lidar_sub, camera_sub);
sync.registerCallback(boost::bind(&calibCallback, _1, _2));
ros::spin();
return 0;
}
0522
- CppLint一个开源脚本用于C++代码风格检查,Python写的通过pip安装
- CLI11命令行解析库:GitHub主页
- 关于ros和glog的版本匹配问题:symbol lookup error: undefined symbol: _ZN6google21kLogSiteUninitializedE;
0525
- 用颜色区分点云深度
#include <iostream>
#include <fstream>
#include <cmath>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
using namespace std;
int main(int argc, char** argv) {
// Load point cloud from file
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPLYFile("input.ply", *cloud);
// Convert depth to color
float max_depth = 0.0;
for (int i = 0; i < cloud->size(); ++i) {
float depth = (*cloud)[i].z;
max_depth = max(max_depth, depth);
}
for (int i = 0; i < cloud->size(); ++i) {
float depth = (*cloud)[i].z;
float intensity = depth / max_depth;
int r = static_cast<int>(255 * intensity);
int b = static_cast<int>(255 * (1 - intensity));
(*cloud)[i].r = r;
(*cloud)[i].g = 0;
(*cloud)[i].b = b;
}
// Save colored point cloud as PLY
pcl::io::savePLYFile("output.ply", *cloud);
return 0;
}
0601
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
bool is_paused = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *viewer_void)
{
if (event.getKeySym() == "space" && event.keyDown())
{
is_paused = !is_paused;
std::cout << (is_paused ? "Paused." : "Resumed.") << std::endl;
}
else if (event.getKeySym() == "Escape" && event.keyDown())
{
std::cout << "Exited." << std::endl;
exit(0);
}
}
int main()
{
pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
viewer.setBackgroundColor(0.0, 0.0, 0.0);
viewer.registerKeyboardCallback(keyboardEventOccurred, nullptr);
int i = 0;
std::string file_prefix = "cloud_";
while (true)
{
// 从文件中读取点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::string file_name = file_prefix + std::to_string(i) + ".pcd";
if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_name, *cloud) == -1)
{
std::cout << "Could not read file: " << file_name << std::endl;
break;
}
// 将点云添加到可视化对象中
viewer.removePointCloud("cloud"); // 先移除之前的点云
viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
// 显示窗口并等待用户交互事件
while (!viewer.wasStopped() && is_paused)
{
viewer.spinOnce();
}
viewer.spinOnce();
i++;
}
return 0;
}
0605
- 相机畸变算法OpenCV
0619
- C++读取json:nlohmann;
- 其他的类似库:TODO
- Python读取json
import json
# 读取json文件
with open('data.json', 'r') as f:
data = json.load(f)
# 修改数据
data['name'] = 'Alice'
data['age'] = 25
# 输出为json文件
with open('new_data.json', 'w') as f:
json.dump(data, f)
0626
std::for_each(v.begin(), v.end(), print)
:对迭代器范围内的每个元素运行function;函数原型Function for_each(InputIterator first, InputIterator last, Function fn)
- 用法
- C++特性语句的参考网址:CPP_reference,cplusplus