//本文通过相机投影坐标对三维点和旋转平移的导数计算,展示ceres的自动微分功能
//具体理论推导参考高博博客 视觉SLAM中的数学基础 第四篇 李群与李代数(2) - 半闲居士 - 博客园
#include <ceres/ceres.h>
#include <ceres/rotation.h>
struct _3D_Error_cst {
double mx;
double my;
_3D_Error_cst(double _x,double _y)
: mx(_x),my(_y)
{
}
template <typename T>
bool operator()(const T* const point, const T *camera, T* residuals) const
{
//for (int i = 0; i < 6; ++i) camera[i] = T(0);
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
T xp = p[0] / p[2];
T yp = p[1] / p[2];
//T camInt[9];
//for (int i = 0; i < 9; ++i) camInt[i] = T(0);
//camInt[0] = T(1);
//camInt[1] = T(1);
//camInt[2] = T(0);
//camInt[3] = T(0);
//const T& l1 = camInt[4];
//const T& l2 = camInt[5];
//const T& p1 = camInt[6];
//const T& p2 = camInt[7];
//const T& l3 = camInt[8];
//T r2 = xp * xp + yp * yp;
//T distortionx = xp * (T(1.0) + r2 * (l1 + l2 * r2 + l3 * r2*r2)) + T(2)*p1*xp*yp + p2 * (r2 + T(2)*xp*xp);
//T distortiony = yp * (T(1.0) + r2 * (l1 + l2 * r2 + l3 * r2*r2)) + T(2)*p2*xp*yp + p1 * (r2 + T(2)*yp*yp);
// //Compute final projected point position.
//const T& focalx = camInt[0];
//const T& focaly = camInt[1];
//const T& cx = camInt[2];
//const T& cy = camInt[3];
//T predicted_x = focalx * distortionx + cx;
//T predicted_y = focaly * distortiony + cy;
The error is the difference between the predicted and observed position.
//residuals[0] = predicted_x - T(mx);
//residuals[1] = predicted_y - T(my);
residuals[0] = xp - T(mx);
residuals[1] = yp - T(my);
return true;
}
};
Eigen::Matrix3d Skew(const Eigen::Vector3d& v)
{
Eigen::Matrix3d ret;
ret <<
0, -v(2), v(1),
v(2), 0, -v(0),
-v(1), v(0), 0;
return ret;
}
Eigen::Vector3d rot_vec_by_ceres(const double* v,const double* rArr) {
double p[3];
ceres::AngleAxisRotatePoint(rArr, v, p);
return Eigen::Vector3d(p[0],p[1],p[2]);
}
void jacubfunc(const double * p3d,const double *rt)
{
Eigen::Vector3d W(p3d[0], p3d[1], p3d[2]);
Eigen::Vector3d rotV(rt[0], rt[1], rt[2]);
Eigen::Vector3d traV(rt[3], rt[4], rt[5]);
Eigen::AngleAxisd rotM(rotV.norm(), rotV.normalized());
//auto rX = rotM * W;
auto rX = rot_vec_by_ceres(p3d, rt);
Eigen::Vector3d X = rX + traV;
Eigen::Matrix<double, 2, 3> Juv_W;
Eigen::Matrix<double, 2, 3> Juv_X;
Eigen::Matrix<double, 2, 6> Juv_RT;
Eigen::Matrix<double, 3, 6> JX_RT;
double x = X[0];
double y = X[1];
double z = X[2];
Juv_X << 1 / z, 0, -x / (z*z),
0, 1 / z, -y / (z*z);
Juv_W = Juv_X * rotM.toRotationMatrix();
JX_RT.block<3, 3>(0, 0) = -Skew(rX);
JX_RT.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity();
Juv_RT = Juv_X * JX_RT;
std::cout << "juv/W" << "\n" << Juv_W << "\n";
std::cout << "juv/rt" << "\n" << Juv_RT << "\n";
std::cout << "err:" << x/z << "," << y/z << "\n";
}
int main() {
_3D_Error_cst* errfuc1 = new _3D_Error_cst(0,0);
ceres::AutoDiffCostFunction<_3D_Error_cst, 2, 3,6> errdiff(errfuc1);
const double cam_r_t[6] = { 0.1,0,0,-0.5,0.6,1 };
const double p3d[3] = { 1,1,5 };
double res[] = { 0,0 };
jacubfunc(p3d,cam_r_t);
//errfuc1(p3d, res);
constexpr int M =2;
constexpr int N1 =2 * 3;
constexpr int N2 = 2 * 6;
double** jac = 0;
jac = new double*[M];
jac[0] = new double[N1];
for (int j = 0; j < N1; ++j) {
jac[0][j] = 0;
}
jac[1] = new double[N2];
for (int j = 0; j < N2; ++j) {
jac[1][j] = 0;
}
const double* const ParaBlk[] = { p3d,cam_r_t };
//double ParaBlk[1][3];
//for (int i = 0; i < 3; ++i) ParaBlk[0][i] = p3d[i];
errdiff.Evaluate((const double* const*)ParaBlk, res, (double**)jac);
printf("err:%lf,%lf\n", res[0], res[1]);
if (jac) {
for (int j = 0; j < N1; ++j) {
if (j == N1 / M)printf("\n");
printf(" %lf ",jac[0][j]);
}
printf("\n");
for (int j = 0; j < N2; ++j) {
if (j == N2 / M)printf("\n");
printf(" %lf ", jac[1][j]);
}
printf("\n");
}
system("pause");
return 0;
}
节可以参考高博的slam教程 直接法 - 半闲居士 - 博客园