核函数:某个数据点的残差过大的时候,降低其权重。以曲线拟合为例,
我们拟合点的时候,显然希望红线而不是黄线,outlier可以抛弃。
残差大于0.1则降低其权重ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
核函数:某个数据点的残差过大的时候,降低其权重。以曲线拟合为例,
我们拟合点的时候,显然希望红线而不是黄线,outlier可以抛弃。
残差大于0.1则降低其权重ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
编译工程时报错 integer_sequence’ is not a member of ‘std’
CMake中添加 set(CMAKE_CXX_FLAGS "-std=c++14")
在CMake3.16版本编译std::make_unique部分的代码,没有报错。在3.10版本上编译却报错了,在CMakeLists.txt加入set(CMAKE_CXX_STANDARD 14)后才成功。
我升级CMake后出现的报警,虽然不影响,但是一旦工程多了以后,这个报警会很烦人。只需要修改cmake_minimum_required (VERSION 3.19.0)
但是有时CMakeLists太多了,不可能一一去修改,使用sed命令集体修改:1
sed -i "s/2.8.3/3.19/g" `grep 2.8.3 -rl . --include="*.txt" `
这个命令不要滥用,否则可能更改过多
1 | CMake Warning (dev) at /usr/local/share/cmake-3.17/Modules/FindPackageHandleStandardArgs.cmake:272 (message): |
在find_package(PCL REQUIRED)之前加上1
2
3if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
endif()
有时明明写好了,但编译会出现报错,看上去是CMakeLists中没有编译规则1
2
3
4
5
6make[2]: *** No rule to make target 'package/CMakeFiles/test_bin.dir/build'。 停止。
CMakeFiles/Makefile2:3192: recipe for target 'package/CMakeFiles/test_bin.dir/all' failed
make[1]: *** [package/CMakeFiles/test_bin.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed
此时再重新编译仍然报错,只要把CMakeLists改一下,再编译就通过了
执行编译时报错:1
CMake Error: The current CMakeCache.txt directory /home/user/common/build/CMakeCache.txt is different than the directory /home/user/space/build where CMakeCache.txt was created. This may result in binaries being created in the wrong place. If you are not sure, reedit the CMakeCache.txt
将当前工程的CMakeCache全删除再编译1
rm $(find -name *.txt | grep CMakeCache)
这是因为在arm平台上不存在SSE指令集的,在x86平台才会有,因此需要在CMakLists文件中把—msse字样的都注释掉
看到网上有些人说标定结果不错,但我试了很多次,开始还能出运行结果,但是和实际差太多,后来换了个相机,几乎不能运行成功了。kalibr图案、棋盘格、apriltag都试过了,代码调试了很多地方,都不顺利,看github也有很多issue未解决,先搁置了
laser_filter的配置filter.yaml修改如下1
2
3
4
5
6
7
8
9
10
11
12scan_filter_chain:
- name: angle
type: laser_filters/LaserScanAngularBoundsFilter
params:
lower_angle: -0.6
upper_angle: 0.6
- name: range
type: laser_filters/LaserScanRangeFilter
params:
use_message_range_limits: false
lower_threshold: 0.5
upper_threshold: 2.2
配置文件中的tag_spacing = 0.3并不是tag_spacing的实际距离大小,而是比例:8.8x0.3=2.64,因此这里的tag_spacing是一个固定值为0.3,不用修改!
录制bag要包括雷达scan和图像1
rosbag record scan_filtered /camera_node/image_raw -O pinhole.bag
roslaunch lasercamcal_ros kalibra_apriltag.launch会把检测到每一帧中的april的信息保存到apriltag_pose.txt文件中,作者录制的数据包有1527 msgs,最终也是每一帧都检测到了,我录制的bag有时会有一些帧检测不到,似乎不是什么问题。
作者也是没有对录制的包直接做数据时间同步的,相机的帧率大概是2D激光雷达的二倍
要求是:标定板在激光正前方120°范围内,并且激光前方2m范围内只存在一个连续的直线线段,所以请在空旷的地方采集数据,不然激光数据会提取错误。需要充分旋转 roll 和 pitch。更直白一点,假设在长方形的标定板中心画了个十字线,那请绕着十字线的两个轴充分旋转,比如绕着竖轴旋转,然后还要绕着横轴旋转。在运行offline程序时,程序会将认为正确的直线会显示为红色。
激光线要能落在标定板上。不断调整标定板姿态,每换一个姿态(绕Roll, Pitch, Yaw旋转,移动位置),请保持静止 2 秒以上,采集 10 个姿态左右的数据(当然越多越好)。用 rosbag 记录所有的图像和激光数据, 标定工具会自动检测你保持静止时刻的数据,并用来标定。
1 | --------- start AutoGetLinePts --------- |
这里的1000,1414其实就是对应scan的range为nan的情况,在if(d1 < range_max && d2 < range_max)里过滤掉了,不用管了
运行roslaunch lasercamcal_ros calibra_offline.launch,结果OpenCV窗口的标定板对应线段不是红色,也没有results.yaml
终端出现Valid Calibra Data too little
查代码发现points = AutoGetLinePts(Points);没有检测到直线。接着查发现问题在selectScanPoints.cpp中的1
2
3
4
5
6
7
8
9 // 至少长于 20 cm, 标定板不能距离激光超过2m, 标定板上的激光点超过 50 个
if(dist.head(2).norm() > 0.2
&& points.at(seg.id_start).head(2).norm() < 2
&& points.at(seg.id_end).head(2).norm() < 2
&& seg.id_end-seg.id_start > 50 )
{
seg.dist = dist.head(2).norm();
segs.push_back(seg);
}
有时第一项和最后一项不符合;有时第一项符合,但中间两项又不符合;第四项大部分情况是3. 常见的四个参数如下1
2
3
4
5
6
7
8
90.100479
1.294
1.218
3
0.201422
2.357
2.513
3
看来是不符合只有一个连续直线这条要求,后来又试了一次,虽然出现了红色直线,但是错的
最后终于明白了,源码中的参数都是作者的雷达相关的,需要根据自己雷达调整。int delta = 80/0.3;的两个参数根据雷达角分辨率调整。50降为18,这样就成功了,int skip = 3;也可能要调整。
运行calibra_offline.launch得到的bug1
2
3
4Load apriltag pose size: 582
terminate called after throwing an instance of 'std::out_of_range'
what(): vector::_M_range_check: __n (which is 18446744073709551615) >= this->size() (which is 57)
[lasercamcal_ros-1] process has died
有时连录几次bag,都出现这种情况,估计是某个地方在for循环里push_back导致
计算初值阶段1
2
3
4
5
6
7
8
9~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Notice Notice Notice: system unobservable !!!!!!!
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
------- Closed-form solution Tlc: -------
1 0 0 -nan
0 1 0 -nan
0 0 1 nan
0 0 0 1
建立tf树 A—->B—->C,求 C—->A变换1
2rosrun tf static_transform_publisher 0.169, 0.035, -0.125, -1.57, 0, -1.57 A B 100
rosrun tf static_transform_publisher 0, 0.015, 0, -1.57, 0, -1.57 B C 100
平时使用的tf是tf::Transformer::lookupTransform(target_frame, source_frame),表示的是 source_frame ---> target_frame的变换,获得的位姿关系是子在父坐标系中,所以是lookupTransform("A", "B"),根据《SLAM十四讲》63页的表示方法,写成
,也就是父子顺序。 因此 C—->A变换是
CMake关键部分:1
2
3
4
5
6
7find_package(catkin REQUIRED COMPONENTS
eigen_conversions
roscpp
tf
pcl_ros
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
c++关键部分1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
//创建一个StampedTransform对象存储变换结果数据
tf::StampedTransform ab;
tf::StampedTransform bc;
tf::StampedTransform tfAC;
try{
listener.lookupTransform("A", "B",
ros::Time(0), ab);
listener.lookupTransform("B", "C",
ros::Time(0), bc);
listener.lookupTransform("A", "C",
ros::Time(0), tfAC);
// 输出 lookupTransform 输出的 C--->A 变换
cout << "X: "<< tfAC.getOrigin().getX() << " Y: "<< tfAC.getOrigin().getY()
<<" Z: "<< tfAC.getOrigin().getZ()<< endl;
cout << "quaternion: "<< tfAC.getRotation().x() <<" " << tfAC.getRotation().y()<<
" " << tfAC.getRotation().z() << " " << tfAC.getRotation().w() << endl;
Eigen::Matrix4f AB, BC, AC;
// 使用 pcl_ros 直接得到 4x4的齐次变换矩阵
pcl_ros::transformAsMatrix(ab, AB);
pcl_ros::transformAsMatrix(bc, BC);
std::cout << "AB:" <<std::endl;
std::cout << AB <<std::endl;
std::cout << "BC:" <<std::endl;
std::cout << BC <<std::endl;
std::cout << "AC:" <<std::endl;
AC = AB * BC;
std::cout << AC <<std::endl;
cout << "-------------------------------------" << endl;
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
}
可以看到AC的右侧平移部分和tfAC.getOrigin()部分相同
tf::lookupTransform的源码一直追溯到tf2_ros::Buffer::lookupTransform,源码在f2/src/buffer_core.cpp,注意其中的accum函数和BufferCore::walkToTopParent函数
tf_eigen.h提供了不少很有用的函数,用于tf和Eigen之间的矩阵和向量互相转换。使用前1
2
3
4find_package(catkin REQUIRED COMPONENTS
eigen_conversions
tf_conversions
)
头文件#include <tf_conversions/tf_eigen.h>
1 | // Converts an Eigen Quaternion into a tf Matrix3x3. |
realsense和雷达外参的粗标定,也就是laser坐标系和camera_color_optical_frame之间的相对位姿,但如果realsense是低着头,竖直和横向的相对位姿的测量误差就大了。
使用realsense检测贴在墙上的二维码,获得距离,同时使用雷达检测到墙的距离,让雷达尽量正对二维码位置。
雷达到前方距离: 1.288,对应realsense的z值: 1.119,二者差 0.169
用直尺测量高度的差: 0.125
横向: realsense在雷达左侧 0.035
如果laser为父坐标系,二者的变换矩阵为
但realsense的坐标系体系是 camera_link —-> camera_color_frame —-> camera_color_optical_frame,其中1
2
3
4
5camera_link camera_color_optical_frame
- Translation: [0.000, 0.015, 0.000]
- Rotation: in Quaternion [-0.498, 0.502, -0.498, 0.502]
in RPY (radian) [-1.571, 0.008, -1.570]
in RPY (degree) [-89.992, 0.476, -89.975]
所以不能直接建立laser坐标系和camera_color_optical_frame的父子关系,要建立laser和camera_link的。
使用变换矩阵的计算,已知 , 。 计算
结果转为tf格式,应当为 <node pkg="tf" type="static_transform_publisher" name="laser_camera_broadcaster" args="0.169 0.02 -0.125 0 0 0 laser camera_link 100"/>
标准化后的向量点乘得到的值为夹角的余弦。如果得到 -1, 0, -1,便可知道两个向量方向的关系是相反, 垂直, 相同
向量叉乘后得到的向量和原先两个向量垂直,也就是法向量
向量叉乘后得到的向量的模,其值为两个向量构成的三角形的面积的二倍。


用行列式的表示方法观察,不用计算就能看出前三条。
高中学过的表示方法为 ,法向量为 。这里涉及到了归一化问题,两边可以乘以系数,还是同一个平面,也可以说法向量是不唯一的。但如果给出了一般式 ,就说法向量为
空间内一点 到平面的距离是 ,如果已经归一化,那么分母就是 1
法向量可以看做平面上两个向量的叉乘。
N为法向量(1x3的向量),那么[N, d]或者(N | d)为 1x4 的向量,其中d为平面到原点的距离。
现在平面的表示形式(无论哪种)都是基于坐标系A,另有一个坐标系B,从B到A的变换矩阵是M(4x4),那么平面在坐标系B下的表示形式是什么?
向量 ,注意不是法向量,平面上一点为 ,那么 ,经过变换后,点v变成了 ,那么有 ,也就有,因此可得 ,M矩阵的逆根据《SLAM十四讲》的公式3.13可以直接写出,最后根据 写成一般式
感觉这样推导并不严谨,但可以这样理解。
最常见的一个例子是 链接,不再复制粘贴
还是之前的曲线拟合问题: ,如果改用解析求导,需要构建一个继承CostFunction的类。核心函数是bool CostFunction: :Evaluate(double const *const *parameters, double *residuals, double **jacobians),计算残差向量和雅可比矩阵
parameters: parameters是一个大小为CostFunction::parameter_block_sizes_.size()(输入参数块的数量和大小)的数组,parameters[i]是一个大小为parameter_block_sizes_[i]的数组,其中包含CostFunction所依赖的第i个参数块。parameters不会为nullptr
residuals: 是一个大小为num_residuals_(输出残差的个数)的数组。residuals也不会为nullptr
jacobians: 是一个大小为CostFunction::parameter_block_sizes_.size()的数组。如果jacobians是nullptr,用户只需要计算残差
jacobians[i]: 是一个大小为 num_residuals x parameter_block_sizes_[i] 的行数组,如果jacobians[i]不为nullptr,用户需要计算关于parameters[i]的残差向量的雅可比矩阵,并将其存储在这个数组中,即
如果jacobians为nullptr,通常我们只需要在Evaluate函数中实现residuals的计算,jacobians后面可以通过Ceres提供的自动求导(数值求导)替代,否则,还需要在Evaluate函数中实现jacobians的计算
如果parameters大小和residuals大小在编译时是已知的,就可以使用SizeCostFunction,该函数可以将paramters大小和residuals大小设置为模板参数,用户只需要在使用的时候给模板参数赋值就可以1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28// 模板参数依次为: number of residuals, first parameter的维度
class MyCostFun : public SizedCostFunction<1, 1, 1, 1>
{
public:
MyCostFun(double x, double y):
m_x(x), m_y(y){}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const
{
double a = parameters[0][0];
double b = parameters[0][1];
double c = parameters[0][2];
residuals[0] = m_y - exp(a*m_x*m_x + b*m_x + c);
if (jacobians != NULL && jacobians[0] != NULL)
{
jacobians[0][0] = -exp(a*m_x*m_x + b*m_x + c)* m_x * m_x;
jacobians[0][1] = -exp(a*m_x*m_x + b*m_x + c)* m_x;
jacobians[0][2] = -exp(a*m_x*m_x + b*m_x + c);
}
return true;
}
protected:
double m_x;
double m_y;
};
拟合 时,使用SizedCostFunction<1, 2>,只计算雅格比jacobians[0][0] 和 jacobians[0][1]
1 | int main(int argv, char** argc) |
一开始我设置代价函数的模板为SizedCostFunction<1, 3>,结果运行报错: [problem_impl.cc:286] Check failed: num_parameter_blocks == cost_function->parameter_block_sizes().size() (3 vs. 1)
问题在于parameter_block_sizes只有1,而我们需要3,也就是对应abc三个参数。读SizedCostFunction源码发现,模板定义为template <int kNumResiduals, int... Ns>,构造函数只有两行1
2set_num_residuals(kNumResiduals);
*mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
因此从第2个模板参数开始,有几个参数就对应parameter_block_sizes,改为SizedCostFunction<1, 1, 1, 1>
添加编译参数,例如:
编译时有时出现报警warning: extra ‘;’ [-Wpedantic],很影响看编译信息,可使用下面方法禁止pedantic1
2
3
4
5
6
7
8
9
10//save compiler switches
//Bad headers with problem goes here
//restore compiler switches
类似的报警还有warning: ISO C++ forbids variable length array ‘angle_compensate_nodes’ [-Wvla],把上面的"-Wpedantic"换成"-Wvla"即可
1 | # 添加docker的GPG公匙 |
docker load -i 本地文件名 把本地文件做成image
执行docker images,出现下面结果
1 | REPOSITORY TAG IMAGE ID CREATED SIZE |
对第一个镜像设置tag: docker tag 15b1a367ae86 ros2:jazzy-full
docker rmi IMAGE_ID 删除镜像docker ps -a 查看所有容器,包括已经退出的,对不需要的可以删除
删除容器: docker rm CONTAINER_ID
docker start ed86d97c09bd 启动已经停止的容器
docker stop ed86d97c09bd 停止容器
docker run --name ubuntu -p 3306:3306 -p 22000:22 -it django_lesson:1.0 -it 表示以交互模式运行容器,通常用于进入容器内部进行操作
进入容器: docker run -it a4c6cd090ec3 bash,但是如果在另一个终端再次运行这个命令,进入的不是同一个容器
如果要在另一个终端进同一个容器,使用 docker exec -it CONTAINER_ID /bin/bash
Dockerfile 和上下文构建镜像。Dockerfile往往是镜像需要安装的一些库和依赖项docker build ., 指定 PATH ,因此本地目录中的所有文件都会被延迟并发送到 Docker 守护程序docker build -f /path/to/a/Dockerfile . 指定dockerfile,不指定的话,默认会读取上下文路径下的 dockerfileGot permission denied while trying to connect to the Docker daemon socket1 | sudo groupadd docker #添加docker用户组 |
先把镜像跑起来,然后从运行起来的容器中复制文件出来,复制命令如下:1
2# 从容器复制文件或目录到宿主机器
docker cp 6619ff360cce:/opt/h2-data/pkslow ./
首先,第一种方法并不是万能的,因为有些镜像过于简单,少了许多基础命令,以至于无法复制文件,也无法进入shell环境。其次,要运行起来再操作,也有点占用资源,比较麻烦。
以ghcr.io/kedacore/keda:2.2.0为例演示如下从该镜像中提取文件:
(1)将镜像保存为tar文件
1 | docker save -o keda.tar ghcr.io/kedacore/keda:2.2.0 |
(2)解压tar文件
1 | tar xvf keda.tar |
可以看到每个分层的信息,我们查看manifest.json可以看到具体哪个layer是最新的。
(3)找其中一个layer再解压
1 | tar xvf ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/layer.tar |
这样,我们就获取到了keda这个可执行文件。每层的打包内容不一样,需要看所需的文件在哪个Layer。