http://ceres-solver.org/nnls_tutorial.html
一.非线性最小二乘
ρ
i
(
∥
f
i
(
x
i
1
,
.
.
.
,
x
i
k
)
∥
2
)
\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)
ρi(∥fi(xi1,...,xik)∥2)是ResidualBlock,
f
i
(
⋅
)
f_i(\cdot)
fi(⋅)是CostFunction，
大多数情况一群小标量一起出现，例如相机的平移位姿和四元数旋转分量，例如
[
x
i
1
,
.
.
.
,
x
i
k
]
\left[x_{i_1},... , x_{i_k}\right]
[xi1,...,xik]是ParameterBlock，也可以是一元的。
ρ
i
\rho_i
ρi是鲁棒核函数
1.1 例1 non-linear least squares problem
1
2
∑
i
∥
f
i
(
x
i
1
,
.
.
.
,
x
i
k
)
∥
2
\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2
21i∑∥fi(xi1,...,xik)∥2
例如最小化：
1
2
(
10
−
x
)
2
.
\frac{1}{2}(10 -x)^2.
21(10−x)2.
1.1.1 step1
write a functor that will evaluate this the function: f ( x ) = 10 − x f(x) = 10 - x f(x)=10−x
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
operator() is a emplated method,允许ceres调用CostFunctor::operator()
T根据需要的类型：
T=double 直接需要数据
T=Jet 需要Jacobians
1.1.2 构建non-linear least squares problem
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// The variable to solve for with its initial value.
double initial_x = 5.0;
double x = initial_x;
// Build the problem.
Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);
// Run the solver!
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}
/*AutoDiffCostFunction将CostFunctor作为输入，
自动对其进行区分，并为其提供一个CostFunction接口。
*/
输出
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.512500e+01 0.00e+00 9.50e+00 0.00e+00 0.00e+00 1.00e+04 0 5.33e-04 3.46e-03
1 4.511598e-07 4.51e+01 9.50e-04 9.50e+00 1.00e+00 3.00e+04 1 5.00e-04 4.05e-03
2 5.012552e-16 4.51e-07 3.17e-08 9.50e-04 1.00e+00 9.00e+04 1 1.60e-05 4.09e-03
Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10
Ceres只在迭代结束时打印出显示，并在检测到收敛时立即终止，这就是为什么这里只看到两次迭代，而不是三次。
2.导数
2.1 Numeric Derivatives(数值导数)
残差调库函数时
用户构建残差，并创建NumericDiffCostFunction例如对于
f
(
x
)
=
10
−
x
f(x) = 10 - x
f(x)=10−x
相应的factor是
struct NumericDiffCostFunctor {
bool operator()(const double* const x, double* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
加入到问题中变为
CostFunction* cost_function =
new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);
建议使用自动微分
2.2 Analytic Derivatives(解析微分)
在某些时候自动微分并不可靠，计算微分解析时比利用链式法则自动求导更有效。
在这种情况下自己提供your own residual and jacobian computation code
如果知道参数和残差的size(),定义CostFunction和SizedCostFunction的子类，以下是这种子类function的实现。
class QuadraticCostFunction : public ceres::SizedCostFunction<1 /* number of residuals */, 1 /* size of first parameter */> {
public:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
const double x = parameters[0][0];
residuals[0] = 10 - x;
// f'(x) = -1. Since there's only 1 parameter and that parameter
// has 1 dimension, there is only 1 element to fill in the
// jacobians.
//
// Since the Evaluate function can be called with the jacobians
// pointer equal to nullptr, the Evaluate function must check to see
// if jacobians need to be computed.
//
// For this simple problem it is overkill to check if jacobians[0]
// is nullptr, but in general when writing more complex
// CostFunctions, it is possible that Ceres may only demand the
// derivatives w.r.t. a subset of the parameter blocks.
// Compute the Jacobian if asked for.
if (jacobians != nullptr && jacobians[0] != nullptr) {
jacobians[0][0] = -1;
}
return true;
}
};
SimpleCostFunction（实现的子类）::Evaluate提供了一个输入参数数组、一个用于残差的输出数组残差和一个用于 Jacobian 的输出数组 jacobians。 jacobians 数组是可选的，Evaluate 需要检查它是否为非空，如果是，则用残差函数的导数值填充它。 在这种情况下，由于残差函数是线性的，雅可比矩阵是常数。
从上面的代码片段可以看出，实现CostFunction对象有点复杂。我们建议，除非您有充分的理由自己管理雅可比计算，否则您可以使用AutoDiffCostFunction或NumericDiffCostFunction来构造剩余块。
2.3 More About Derivatives
3 Powell’s Function
error
static Eigen::Matrix<double,3,3> skew(Eigen::Matrix<double,3,1>& mat_in){ // 反对称矩阵定义
Eigen::Matrix<double,3,3> skew_mat;
skew_mat.setZero();
skew_mat(0,1) = -mat_in(2);
skew_mat(0,2) = mat_in(1);
skew_mat(1,2) = -mat_in(0);
skew_mat(1,0) = mat_in(2);
skew_mat(2,0) = -mat_in(1);
skew_mat(2,1) = mat_in(0);
return skew_mat;
}
class PlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3>{
public:
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
PlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),last_point_m(last_point_m_), s(s_){}
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians)const { // 定义残差模型
// 叉乘运算， j,l,m 三个但构成的平行四边面积(摸)和该面的单位法向量(方向)
Eigen::Vector3d ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
ljm_norm.normalize(); // 单位法向量
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d lp; // “从当前阵的当前点” 经过转换矩阵转换到“上一阵的同线束激光点”
Eigen::Vector3d lp_r = q_last_curr * curr_point ; // for compute jacobian o rotation L: dp_dr
lp = q_last_curr * curr_point + t_last_curr;
// 残差函数
double phi1 = (lp - last_point_j ).dot(ljm_norm);
residuals[0] = std::fabs(phi1);
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
phi1 = phi1 / residuals[0];
// Rotation
Eigen::Matrix3d skew_lp_r = skew(lp_r);
Eigen::Matrix3d dp_dr;
dp_dr.block<3,3>(0,0) = -skew_lp_r;
Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor>> J_so3_r(jacobians[0]);
J_so3_r.setZero();
J_so3_r.block<1,3>(0,0) = phi1 * ljm_norm.transpose() * (dp_dr);
Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>> J_so3_t(jacobians[1]);
J_so3_t.block<1,3>(0,0) = phi1 * ljm_norm.transpose();
}
}
return true;
}
};
如果不加static
/usr/bin/ld: CMakeFiles/aloam_laser_odometry_node.dir/src/models/loam/aloam_registration.cpp.o: in function `skew(Eigen::Matrix<double, 3, 1, 0, 3, 1>&)':
aloam_registration.cpp:(.text+0x0): multiple definition of `skew(Eigen::Matrix<double, 3, 1, 0, 3, 1>&)'; CMakeFiles/aloam_laser_odometry_node.dir/src/aloam_laser_odometry_node.cpp.o:aloam_laser_odometry_node.cpp:(.text+0x2f0): first defined here
collect2: error: ld returned 1 exit status
make[2]: *** [lidar_localization/CMakeFiles/aloam_laser_odometry_node.dir/build.make:762: /home/quanchao/shenlanxueyuan/sensorfusion4/PA3/devel/lib/lidar_localization/aloam_laser_odometry_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:590: lidar_localization/CMakeFiles/aloam_laser_odometry_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
扫一扫
775
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
