feat:给solver添加中文注释
parent
9bf19d5ff3
commit
3c4963d289
|
|
@ -7,99 +7,234 @@
|
|||
|
||||
namespace oh_my_loam {
|
||||
|
||||
// ============================================================================
|
||||
// 数据结构定义
|
||||
// ============================================================================
|
||||
|
||||
/**
|
||||
* @brief 点-直线匹配对数据结构
|
||||
*
|
||||
* 包含一个待优化的点和对应的直线。直线由直线上任意两个点定义。
|
||||
*/
|
||||
struct PointLinePair {
|
||||
TPoint pt;
|
||||
TPoint pt; // 待优化的点
|
||||
|
||||
// 内部结构体，用于描述直线
|
||||
struct Line {
|
||||
TPoint pt1, pt2;
|
||||
TPoint pt1, pt2; // 定义直线的两个点
|
||||
|
||||
// 默认构造函数
|
||||
Line() = default;
|
||||
|
||||
// 构造函数，根据传入的两个点构造直线
|
||||
Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
|
||||
};
|
||||
Line line;
|
||||
|
||||
Line line; // 与点对应的直线
|
||||
|
||||
// 构造函数，根据点和直线结构体构造匹配对
|
||||
PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
|
||||
|
||||
// 构造函数，根据点和直线上的两个点构造匹配对
|
||||
PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
|
||||
: pt(pt), line(pt1, pt2) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 点-平面对匹配对数据结构
|
||||
*
|
||||
* 包含一个待优化的点和对应的平面。平面由平面内任意三个点确定。
|
||||
*/
|
||||
struct PointPlanePair {
|
||||
TPoint pt;
|
||||
TPoint pt; // 待优化的点
|
||||
|
||||
// 内部结构体，用于描述平面
|
||||
struct Plane {
|
||||
TPoint pt1, pt2, pt3;
|
||||
TPoint pt1, pt2, pt3; // 定义平面的三个点
|
||||
|
||||
// 默认构造函数
|
||||
Plane() = default;
|
||||
|
||||
// 构造函数，根据三个点构造平面
|
||||
Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
|
||||
: pt1(pt1), pt2(pt2), pt3(pt3) {}
|
||||
};
|
||||
Plane plane;
|
||||
|
||||
Plane plane; // 与点对应的平面
|
||||
|
||||
// 构造函数，根据点和平面结构体构造匹配对
|
||||
PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
|
||||
|
||||
// 构造函数，根据点和平面上的三个点构造匹配对
|
||||
PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
|
||||
const TPoint &pt3)
|
||||
: pt(pt), plane(pt1, pt2, pt3) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-直线匹配对数据结构
|
||||
*
|
||||
* 包含一个待优化的点和对应的直线系数，系数存储在 6×1 的向量中，
|
||||
* 前 3 个元素表示直线上一个点，后 3 个元素表示直线方向向量。
|
||||
*/
|
||||
struct PointLineCoeffPair {
|
||||
TPoint pt;
|
||||
Eigen::Matrix<double, 6, 1> line_coeff;
|
||||
TPoint pt; // 待优化的点
|
||||
Eigen::Matrix<double, 6, 1> line_coeff; // 直线系数（6维向量）
|
||||
|
||||
// 构造函数，根据点和直线系数构造匹配对
|
||||
PointLineCoeffPair(const TPoint &pt,
|
||||
const Eigen::Matrix<double, 6, 1> &line_coeff)
|
||||
: pt(pt), line_coeff(line_coeff) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-平面对匹配对数据结构
|
||||
*
|
||||
* 包含一个待优化的点和对应的平面系数，系数存储在 4 维向量中，
|
||||
* 通常表示平面方程 ax+by+cz+d=0 的系数。
|
||||
*/
|
||||
struct PointPlaneCoeffPair {
|
||||
TPoint pt;
|
||||
Eigen::Vector4d plane_coeff;
|
||||
TPoint pt; // 待优化的点
|
||||
Eigen::Vector4d plane_coeff; // 平面系数（4维向量）
|
||||
|
||||
// 构造函数，根据点和平面系数构造匹配对
|
||||
PointPlaneCoeffPair(const TPoint &pt, const Eigen::Vector4d &plane_coeff)
|
||||
: pt(pt), plane_coeff(plane_coeff) {}
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// 成本函数定义
|
||||
// ============================================================================
|
||||
|
||||
/**
|
||||
* @brief 点-直线代价函数
|
||||
*
|
||||
* 使用自动微分方式计算代价，用于衡量待优化点到直线的距离误差，
|
||||
* 误差由构成直线的两个点确定。
|
||||
*/
|
||||
class PointLineCostFunction {
|
||||
public:
|
||||
// 构造函数，传入匹配对和时间因子
|
||||
PointLineCostFunction(const PointLinePair &pair, double time)
|
||||
: pair_(pair), time_(time){};
|
||||
: pair_(pair), time_(time) {};
|
||||
|
||||
/**
|
||||
* @brief 重载函数调用操作符
|
||||
*
|
||||
* 模板化实现，用于 Ceres 自动微分，计算当前参数下的残差。
|
||||
*
|
||||
* @param r_quat 旋转部分参数（四元数，长度为4）
|
||||
* @param t_vec 平移部分参数（向量，长度为3）
|
||||
* @param residual 输出残差数组（3个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool operator()(const T *const r_quat, const T *const t_vec,
|
||||
T *residual) const;
|
||||
|
||||
/**
|
||||
* @brief 静态创建函数
|
||||
*
|
||||
* 创建一个自动微分类型的 Ceres 成本函数，并传入当前匹配对和时间因子。
|
||||
*
|
||||
* @param pair 点-直线匹配对
|
||||
* @param time 时间因子，用于时间插值（运动补偿）
|
||||
* @return ceres::CostFunction* 创建的成本函数指针
|
||||
*/
|
||||
static ceres::CostFunction *Create(const PointLinePair &pair, double time) {
|
||||
return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>(
|
||||
new PointLineCostFunction(pair, time));
|
||||
}
|
||||
|
||||
private:
|
||||
PointLinePair pair_;
|
||||
double time_;
|
||||
PointLinePair pair_; // 当前匹配对
|
||||
double time_; // 时间因子
|
||||
|
||||
// 禁止拷贝构造函数和赋值操作（宏定义，确保对象不可复制）
|
||||
DISALLOW_COPY_AND_ASSIGN(PointLineCostFunction)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 点-平面代价函数
|
||||
*
|
||||
* 使用自动微分方式计算代价，用于衡量待优化点到平面的距离误差，
|
||||
* 平面由平面内的三个点确定。
|
||||
*/
|
||||
class PointPlaneCostFunction {
|
||||
public:
|
||||
// 构造函数，传入匹配对和时间因子
|
||||
PointPlaneCostFunction(const PointPlanePair &pair, double time)
|
||||
: pair_(pair), time_(time){};
|
||||
: pair_(pair), time_(time) {};
|
||||
|
||||
/**
|
||||
* @brief 重载函数调用操作符
|
||||
*
|
||||
* 模板化实现，用于计算当前参数下的残差（标量残差）。
|
||||
*
|
||||
* @param r_quat 旋转四元数参数（长度为4）
|
||||
* @param t_vec 平移向量参数（长度为3）
|
||||
* @param residual 输出残差（单个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool operator()(const T *const r_quat, const T *const t_vec,
|
||||
T *residual) const;
|
||||
|
||||
/**
|
||||
* @brief 静态创建函数
|
||||
*
|
||||
* 创建一个自动微分类型的成本函数，用于点-平面匹配。
|
||||
*
|
||||
* @param pair 点-平面对匹配对
|
||||
* @param time 时间因子
|
||||
* @return ceres::CostFunction* 成本函数指针
|
||||
*/
|
||||
static ceres::CostFunction *Create(const PointPlanePair &pair, double time) {
|
||||
return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
|
||||
new PointPlaneCostFunction(pair, time));
|
||||
}
|
||||
|
||||
private:
|
||||
PointPlanePair pair_;
|
||||
double time_;
|
||||
PointPlanePair pair_; // 当前匹配对
|
||||
double time_; // 时间因子
|
||||
|
||||
DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-直线代价函数
|
||||
*
|
||||
* 用于含有额外直线系数信息的匹配对，计算待优化点到直线的距离误差。
|
||||
*/
|
||||
class PointLineCoeffCostFunction {
|
||||
public:
|
||||
// 构造函数，传入匹配对和时间因子
|
||||
PointLineCoeffCostFunction(const PointLineCoeffPair &pair, double time)
|
||||
: pair_(pair), time_(time){};
|
||||
: pair_(pair), time_(time) {};
|
||||
|
||||
/**
|
||||
* @brief 重载函数调用操作符
|
||||
*
|
||||
* 模板化实现，计算当前参数下的残差（3维向量残差）。
|
||||
*
|
||||
* @param r_quat 旋转参数（四元数，长度为4）
|
||||
* @param t_vec 平移参数（向量，长度为3）
|
||||
* @param residual 输出残差（3个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool operator()(const T *const r_quat, const T *const t_vec,
|
||||
T *residual) const;
|
||||
|
||||
/**
|
||||
* @brief 静态创建函数
|
||||
*
|
||||
* 创建自动微分类型的成本函数，用于带系数的点-直线匹配。
|
||||
*
|
||||
* @param pair 带系数的点-直线匹配对
|
||||
* @param time 时间因子
|
||||
* @return ceres::CostFunction* 成本函数指针
|
||||
*/
|
||||
static ceres::CostFunction *Create(const PointLineCoeffPair &pair,
|
||||
double time) {
|
||||
return new ceres::AutoDiffCostFunction<PointLineCoeffCostFunction, 1, 4, 3>(
|
||||
|
|
@ -107,20 +242,46 @@ class PointLineCoeffCostFunction {
|
|||
}
|
||||
|
||||
private:
|
||||
PointLineCoeffPair pair_;
|
||||
double time_;
|
||||
PointLineCoeffPair pair_; // 当前匹配对（含直线系数）
|
||||
double time_; // 时间因子
|
||||
|
||||
DISALLOW_COPY_AND_ASSIGN(PointLineCoeffCostFunction)
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-平面代价函数
|
||||
*
|
||||
* 用于含有额外平面系数信息的匹配对，计算待优化点到平面的距离误差。
|
||||
*/
|
||||
class PointPlaneCoeffCostFunction {
|
||||
public:
|
||||
// 构造函数，传入匹配对和时间因子
|
||||
PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &pair, double time)
|
||||
: pair_(pair), time_(time){};
|
||||
: pair_(pair), time_(time) {};
|
||||
|
||||
/**
|
||||
* @brief 重载函数调用操作符
|
||||
*
|
||||
* 模板化实现，计算当前参数下的残差（标量残差）。
|
||||
*
|
||||
* @param r_quat 旋转参数（四元数，长度为4）
|
||||
* @param t_vec 平移参数（向量，长度为3）
|
||||
* @param residual 输出残差（单个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool operator()(const T *const r_quat, const T *const t_vec,
|
||||
T *residual) const;
|
||||
|
||||
/**
|
||||
* @brief 静态创建函数
|
||||
*
|
||||
* 创建自动微分类型的成本函数，用于带系数的点-平面匹配。
|
||||
*
|
||||
* @param pair 带系数的点-平面对匹配对
|
||||
* @param time 时间因子
|
||||
* @return ceres::CostFunction* 成本函数指针
|
||||
*/
|
||||
static ceres::CostFunction *Create(const PointPlaneCoeffPair &pair,
|
||||
double time) {
|
||||
return new ceres::AutoDiffCostFunction<PointPlaneCoeffCostFunction, 1, 4,
|
||||
|
|
@ -129,78 +290,144 @@ class PointPlaneCoeffCostFunction {
|
|||
}
|
||||
|
||||
private:
|
||||
PointPlaneCoeffPair pair_;
|
||||
double time_;
|
||||
PointPlaneCoeffPair pair_; // 当前匹配对（含平面系数）
|
||||
double time_; // 时间因子
|
||||
|
||||
DISALLOW_COPY_AND_ASSIGN(PointPlaneCoeffCostFunction)
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// 成本函数实现（模板函数）
|
||||
// ============================================================================
|
||||
|
||||
/**
|
||||
* @brief 点-直线代价函数的模板实现
|
||||
*
|
||||
* 对于待优化的点，先通过当前位姿（旋转和平移）对其进行变换，
|
||||
* 然后计算变换后的点到直线（由两个点确定）的距离误差，
|
||||
* 此误差以三维残差的形式返回（利用交叉乘积计算三角形面积后除以直线基底长度）。
|
||||
*
|
||||
* @tparam T 数值类型模板参数（如 float 或 double）
|
||||
* @param r_quat 旋转参数（四元数，长度为4）
|
||||
* @param t_vec 平移参数（向量，长度为3）
|
||||
* @param residual 输出残差数组（3个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool PointLineCostFunction::operator()(const T *const r_quat,
|
||||
const T *const t_vec,
|
||||
T *residual) const {
|
||||
// 提取匹配对中的点和直线上的两个点
|
||||
const auto &pt = pair_.pt, &pt1 = pair_.line.pt1, &pt2 = pair_.line.pt2;
|
||||
// 将点转换为 Eigen 向量（3维）
|
||||
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
|
||||
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
|
||||
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
|
||||
|
||||
// 根据输入的旋转参数构造四元数，注意：Ceres中四元数存储顺序可能为 [x,y,z,w]
|
||||
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
|
||||
// 构造平移向量
|
||||
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
|
||||
if (time_ <= 1.0 - 1.0e-6) {
|
||||
// 如果时间因子小于 1，则对旋转和平移进行插值（运动补偿）
|
||||
if (time_ <= T(1.0) - T(1.0e-6)) {
|
||||
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
|
||||
t = T(time_) * t;
|
||||
}
|
||||
// 计算变换后的点位置
|
||||
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
|
||||
|
||||
// norm of cross product: triangle area x 2
|
||||
Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
|
||||
// 利用交叉乘积计算三角形面积（面积 = 0.5 * |(p0-p1) x (p0-p2)|）
|
||||
Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * T(0.5);
|
||||
// 计算直线基底的长度（两点之间的距离）
|
||||
T base_length = (p2 - p1).norm();
|
||||
// 将面积除以基底长度，即可得到点到直线的距离（与三角形高等价）
|
||||
// 注意：这里残差为3维，分别保存了每个坐标轴上的误差（也可看作向量形式）
|
||||
residual[0] = area[0] / base_length;
|
||||
residual[1] = area[1] / base_length;
|
||||
residual[2] = area[2] / base_length;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 点-平面代价函数的模板实现
|
||||
*
|
||||
* 对于待优化的点，通过当前位姿对其进行变换，
|
||||
* 然后计算变换后的点到平面（由三个点确定）的距离，
|
||||
* 平面法向量由三个点计算得到，残差为点到平面的距离（标量）。
|
||||
*
|
||||
* @tparam T 数值类型模板参数
|
||||
* @param r_quat 旋转参数（长度为4的四元数）
|
||||
* @param t_vec 平移参数（3维向量）
|
||||
* @param residual 输出残差（单个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool PointPlaneCostFunction::operator()(const T *const r_quat,
|
||||
const T *const t_vec,
|
||||
T *residual) const {
|
||||
// 提取匹配对中的点和平面上三个点
|
||||
const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
|
||||
&pt3 = pair_.plane.pt3;
|
||||
// 将各点转换为 Eigen 向量
|
||||
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
|
||||
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
|
||||
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
|
||||
Eigen::Matrix<T, 3, 1> p3(T(pt3.x), T(pt3.y), T(pt3.z));
|
||||
|
||||
// 构造旋转四元数和平移向量
|
||||
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
|
||||
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
|
||||
if (time_ <= 1.0 - 1.0e-6) {
|
||||
// 进行运动补偿插值
|
||||
if (time_ <= T(1.0) - T(1.0e-6)) {
|
||||
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
|
||||
t = T(time_) * t;
|
||||
}
|
||||
// 计算变换后的点位置
|
||||
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
|
||||
|
||||
// 计算平面法向量：由平面内两个向量叉乘后归一化得到
|
||||
Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
|
||||
// 计算点到平面的距离（点与平面上一点连线与法向量的内积）
|
||||
residual[0] = (p0 - p1).dot(normal);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-直线代价函数的模板实现
|
||||
*
|
||||
* 使用额外的直线系数信息，将系数向量分为两部分：前 3 个元素为直线上某一点，
|
||||
* 后 3 个元素为直线方向。对待优化的点进行变换后，计算其到直线的距离误差，
|
||||
* 该误差以3维残差输出（利用叉积计算）。
|
||||
*
|
||||
* @tparam T 数值类型模板参数
|
||||
* @param r_quat 旋转参数（四元数，长度为4）
|
||||
* @param t_vec 平移参数（向量，长度为3）
|
||||
* @param residual 输出残差（3个元素）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
|
||||
const T *const t_vec,
|
||||
T *residual) const {
|
||||
// 将待优化点转换为 Eigen 向量
|
||||
Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
|
||||
// 将直线系数转换为模板类型，并分解为直线上的一点 p1 和方向向量 dir
|
||||
Eigen::Matrix<T, 6, 1> coeff = pair_.line_coeff.template cast<T>();
|
||||
Eigen::Matrix<T, 3, 1> p1 = coeff.topRows(3);
|
||||
Eigen::Matrix<T, 3, 1> dir = coeff.bottomRows(3);
|
||||
|
||||
// 构造旋转和平移参数
|
||||
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
|
||||
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
|
||||
if (time_ <= 1.0 - 1.0e-6) {
|
||||
// 如果时间因子小于1，则进行插值
|
||||
if (time_ <= T(1.0) - T(1.0e-6)) {
|
||||
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
|
||||
t = T(time_) * t;
|
||||
}
|
||||
// 计算变换后的点位置
|
||||
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
|
||||
|
||||
// 计算待优化点到直线的距离误差：计算 (p0-p1) 与直线方向的叉积
|
||||
Eigen::Matrix<T, 3, 1> cross = (p0 - p1).cross(dir);
|
||||
residual[0] = cross[0];
|
||||
residual[1] = cross[1];
|
||||
|
|
@ -208,21 +435,39 @@ bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
|
|||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 带系数的点-平面代价函数的模板实现
|
||||
*
|
||||
* 利用传入的平面系数（4维向量，表示平面方程 ax+by+cz+d=0），
|
||||
* 对待优化的点进行变换后计算其到平面的距离残差，残差为标量形式。
|
||||
*
|
||||
* @tparam T 数值类型模板参数
|
||||
* @param r_quat 旋转参数（四元数，长度为4）
|
||||
* @param t_vec 平移参数（向量，长度为3）
|
||||
* @param residual 输出残差（单个标量）
|
||||
* @return true 表示计算成功
|
||||
*/
|
||||
template <typename T>
|
||||
bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
|
||||
const T *const t_vec,
|
||||
T *residual) const {
|
||||
// 将待优化点转换为 Eigen 向量
|
||||
Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
|
||||
// 将平面系数转换为模板类型的 4 维向量
|
||||
Eigen::Matrix<T, 4, 1> coeff = pair_.plane_coeff.template cast<T>();
|
||||
|
||||
// 构造旋转四元数和平移向量
|
||||
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
|
||||
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
|
||||
if (time_ <= 1.0 - 1.0e-6) {
|
||||
// 对时间因子小于1的情况进行插值
|
||||
if (time_ <= T(1.0) - T(1.0e-6)) {
|
||||
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
|
||||
t = T(time_) * t;
|
||||
}
|
||||
// 计算变换后的点位置
|
||||
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
|
||||
|
||||
// 计算点到平面的距离残差：将点 p0 代入平面方程，得到 ax+by+cz+d
|
||||
residual[0] = coeff.topRows(3).transpose() * p0;
|
||||
residual[0] += coeff(3);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -8,58 +8,138 @@
|
|||
|
||||
namespace oh_my_loam {
|
||||
|
||||
// 定义一个局部变量，表示 Huber 损失的尺度参数
|
||||
namespace {
|
||||
double kHuberLossScale = 0.1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief PoseSolver 构造函数
|
||||
*
|
||||
* 初始化位姿求解器，传入初始位姿（包括旋转和位移），并将其复制到内部参数数组中。
|
||||
* 同时为 Ceres 问题添加参数块，其中旋转部分使用 Eigen 四元数参数化，
|
||||
* 并设置 Huber 损失函数用于鲁棒优化。
|
||||
*
|
||||
* @param pose 初始位姿（类型为 common::Pose3d），包含旋转（四元数）和位移向量
|
||||
*/
|
||||
PoseSolver::PoseSolver(const common::Pose3d &pose) {
|
||||
// 将输入位姿的旋转部分（四元数的系数）复制到内部数组 r_quat_ 中，四元数有 4 个元素
|
||||
std::copy_n(pose.r_quat().coeffs().data(), 4, r_quat_);
|
||||
// 将输入位姿的平移向量复制到内部数组 t_vec_ 中，平移向量有 3 个元素
|
||||
std::copy_n(pose.t_vec().data(), 3, t_vec_);
|
||||
// 创建一个 Ceres 的 Huber 损失函数对象，参数 kHuberLossScale 控制损失函数的尺度
|
||||
loss_function_ = new ceres::HuberLoss(kHuberLossScale);
|
||||
problem_.AddParameterBlock(r_quat_, 4,
|
||||
new ceres::EigenQuaternionParameterization());
|
||||
// 向问题中添加旋转参数块，使用 Eigen 四元数参数化以确保单位四元数的约束
|
||||
problem_.AddParameterBlock(r_quat_, 4, new ceres::EigenQuaternionParameterization());
|
||||
// 向问题中添加平移参数块（3个自由度）
|
||||
problem_.AddParameterBlock(t_vec_, 3);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Solve 求解器入口函数
|
||||
*
|
||||
* 利用 Ceres 求解器求解当前的位姿优化问题。设定线性求解器类型、最大迭代次数等参数，
|
||||
* 求解后更新内部的旋转和平移参数，同时将最新位姿保存到 pose 输出参数中。
|
||||
*
|
||||
* @param max_iter_num 最大迭代次数
|
||||
* @param verbose 是否输出详细日志信息
|
||||
* @param pose 求解后得到的最新位姿（输出参数）
|
||||
* @return bool 如果优化收敛则返回 true，否则返回 false
|
||||
*/
|
||||
bool PoseSolver::Solve(int max_iter_num, bool verbose,
|
||||
common::Pose3d *const pose) {
|
||||
// 配置 Ceres 求解选项
|
||||
ceres::Solver::Options options;
|
||||
options.linear_solver_type = ceres::DENSE_QR;
|
||||
options.max_num_iterations = max_iter_num;
|
||||
options.minimizer_progress_to_stdout = false;
|
||||
ceres::Solver::Summary summary;
|
||||
options.linear_solver_type = ceres::DENSE_QR; // 使用密集 QR 分解作为线性求解器
|
||||
options.max_num_iterations = max_iter_num; // 最大迭代次数
|
||||
options.minimizer_progress_to_stdout = false; // 是否将求解进度输出到标准输出
|
||||
ceres::Solver::Summary summary; // 用于存储求解过程和结果的摘要信息
|
||||
// 调用 Ceres 求解器求解问题
|
||||
ceres::Solve(options, &problem_, &summary);
|
||||
// 如果 verbose 为 true，则输出求解摘要报告
|
||||
AINFO_IF(verbose) << summary.BriefReport();
|
||||
// 如果 pose 非空，则将当前求解得到的旋转和平移参数构造为 common::Pose3d 对象输出
|
||||
if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
|
||||
// 返回求解是否收敛的状态（Ceres 的 termination_type 为 CONVERGENCE 表示收敛）
|
||||
return summary.termination_type == ceres::CONVERGENCE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加点-直线匹配项
|
||||
*
|
||||
* 根据输入的点-直线匹配对 pair 以及时间参数 time，
|
||||
* 构造一个对应的代价函数（CostFunction），并将其加入到 Ceres 优化问题中。
|
||||
*
|
||||
* @param pair 点-直线匹配对，包含原始点和匹配直线上的两个点
|
||||
* @param time 时间参数，用于对匹配代价进行时间权重调整
|
||||
*/
|
||||
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
|
||||
// 通过静态工厂函数创建点-直线匹配的代价函数
|
||||
ceres::CostFunction *cost_function =
|
||||
PointLineCostFunction::Create(pair, time);
|
||||
// 将代价函数添加到问题中，设置损失函数（Huber 损失）和待优化参数（旋转和平移）
|
||||
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加点-平面对匹配项
|
||||
*
|
||||
* 根据输入的点-平面对匹配 pair 以及时间参数 time，
|
||||
* 构造一个对应的代价函数（CostFunction），并将其加入到 Ceres 优化问题中。
|
||||
*
|
||||
* @param pair 点-平面对匹配对，包含原始点和匹配平面上的三个点
|
||||
* @param time 时间参数，用于对匹配代价进行时间权重调整
|
||||
*/
|
||||
void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
|
||||
// 创建点-平面对代价函数
|
||||
ceres::CostFunction *cost_function =
|
||||
PointPlaneCostFunction::Create(pair, time);
|
||||
// 添加残差块到优化问题中，待优化参数为旋转和平移
|
||||
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加带有系数信息的点-直线匹配项
|
||||
*
|
||||
* 针对带有额外系数信息的点-直线匹配对，构造对应的代价函数，并加入优化问题中。
|
||||
*
|
||||
* @param pair 点-直线匹配对（含系数信息）
|
||||
* @param time 时间参数
|
||||
*/
|
||||
void PoseSolver::AddPointLineCoeffPair(const PointLineCoeffPair &pair,
|
||||
double time) {
|
||||
// 通过工厂函数创建带系数的点-直线代价函数
|
||||
ceres::CostFunction *cost_function =
|
||||
PointLineCoeffCostFunction::Create(pair, time);
|
||||
// 将代价函数加入到问题中
|
||||
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加带有系数信息的点-平面对匹配项
|
||||
*
|
||||
* 针对带有额外系数信息的点-平面对匹配对，构造对应的代价函数，并加入优化问题中。
|
||||
*
|
||||
* @param pair 点-平面对匹配对（含系数信息）
|
||||
* @param time 时间参数
|
||||
*/
|
||||
void PoseSolver::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair,
|
||||
double time) {
|
||||
// 创建带系数的点-平面代价函数
|
||||
ceres::CostFunction *cost_function =
|
||||
PointPlaneCoeffCostFunction::Create(pair, time);
|
||||
// 将代价函数添加到 Ceres 问题中，优化参数为旋转和平移
|
||||
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取当前求解的位姿
|
||||
*
|
||||
* 根据内部存储的旋转和平移参数构造一个 common::Pose3d 对象，
|
||||
* 用于返回当前的位姿估计结果。
|
||||
*
|
||||
* @return common::Pose3d 当前位姿
|
||||
*/
|
||||
common::Pose3d PoseSolver::GetPose() const {
|
||||
return common::Pose3d(r_quat_, t_vec_);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue