SLAM——之ceres函数库: ceres优化相机位姿求解
·
求解结果如下:




mat 初始化,eigenvalue初始化
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
Eigen::Matrix<float,3,1> vd_3d;
v_3d << 3, 2, 1;
求解目标函数结构体构造与实例
struct CurveFittingCost {
CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}
template<typename T>
bool operator() (
const T *const camera_pose,
T *residual)const
{
T point[3];
point[0] = T(_point3d.depth_x);
point[1] = T(_point3d.depth_x);
point[2] = T(_point3d.depth_z);
T p[3];
ceres::AngleAxisRotatePoint(camera_pose, point, p);
p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];
//p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
T x_p = p[0] / p[2];
T y_p = p[1] / p[2];
T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));
T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));
residual[0] = T(_point2d.x) - predicted_x;
residual[1] = T(_point2d.y) - predicted_y;
return true;
}
static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {
return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
new CurveFittingCost(point2d, point3d)));
}
const Point2d _point2d;
const Point3f _point3d;
};
struct CurveFittingCost {
CurveFittingCost(Point2f point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}
template<typename T>
bool operator() (
const T *const camera_pose,
T *residual)const
{
T point[3];
point[0] = T(_point3d.depth_x);
point[1] = T(_point3d.depth_y);
point[2] = T(_point3d.depth_z);
T p[3];
ceres::AngleAxisRotatePoint(camera_pose, point, p);
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
T predicted_x = p[0] / p[2] * camera_matrix(0,0) + camera_matrix(0,2);
T predicted_y = p[1] / p[2] * camera_matrix(1,1) + camera_matrix(1,2);
residual[0] = T(_point2d.x) - predicted_x;
residual[1] = T(_point2d.y) - predicted_y;
return true;
}
static ceres::CostFunction* Create(const Point2f point2d, const Point3f point3d) {
return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
new CurveFittingCost(point2d, point3d)));
}
const Point2f _point2d;
const Point3f _point3d;
};
struct PnPCeres
{
PnPCeres(Point2f uv, Point3f xyz) : _uv(uv), _xyz(xyz) {}
// 残差的计算
template <typename T>
bool operator() (
const T *const camera, // 位姿参数,有6维
T* residual) const // 残差
{
T p[3];
T point[3];
point[0] = T(_xyz.x);
point[1] = T(_xyz.y);
point[2] = T(_xyz.z);
ceres::AngleAxisRotatePoint(camera, point, p);//计算RP
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
T xp = p[0] / p[2];
T yp = p[1] / p[2];//xp,yp是归一化坐标,深度为p[2]
T u_ = xp*camera_matrix(0, 0) + camera_matrix(0, 2);
T v_ = yp*camera_matrix(1, 1) + camera_matrix(1, 2);
residual[0] = T(_uv.x) - u_;
residual[1] = T(_uv.y) - v_;
return true;
}
static ceres::CostFunction* Create(const Point2f uv, const Point3f xyz) {
return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>(
new PnPCeres(uv, xyz)));
}
const Point2f _uv;
const Point3f _xyz;
};
目标函数调用方式
struct CurveFittingCost {
CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}
template<typename T>
bool operator() (
const T *const camera_pose,
T *residual)const
{
T point[3];
point[0] = T(_point3d.depth_x);
point[1] = T(_point3d.depth_x);
point[2] = T(_point3d.depth_z);
T p[3];
ceres::AngleAxisRotatePoint(camera_pose, point, p);
p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];
//p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
T x_p = p[0] / p[2];
T y_p = p[1] / p[2];
T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));
T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));
residual[0] = T(_point2d.x) - predicted_x;
residual[1] = T(_point2d.y) - predicted_y;
return true;
}
static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {
return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
new CurveFittingCost(point2d, point3d)));
}
const Point2d _point2d;
const Point3f _point3d;
};
更多推荐
所有评论(0)