第4问 求导公式推导，如下图所示
-----------------------------------------接着上面的问题-----------------------------------------------
利用任务五的8点法求解出的相机Pose和点的3D坐标作为初值，构建BA问题，使用Ceres对相机位姿和点的坐标做进一步优化
1. 使用Ceres自动求导方法实现, 并解释Ceres自动求导的基本原理
2. 使用自定义雅克比矩阵方法实现，并和自动求导方式做计算效率对比
3. 使用 gradient check 检查自己的雅克比矩阵是否正确
-----------------------------------------接着上面的问题-----------------------------------------------
-----------------------------------------已知的3D points如下-----------------------------------------
Xw Yw Zw
-1.24167 0.415513 5.26715
-1.1036 -0.630794 5.42475
-4.71247 -0.453318 6.73188
0.585758 0.345869 1.82821
0.563493 0.344662 1.76335
-1.17131 -0.679662 5.63624
-2.28384 -0.113673 5.36991
0.567199 0.336969 1.77459
-1.10779 0.400221 5.48252
-1.22798 -0.663676 5.73312
0.841496 -0.0921831 4.02343
-2.62935 0.13478 5.08306
-2.64879 0.140631 5.13771
0.830696 -0.0902273 3.96377
-2.2986 0.0407816 5.30456
-2.66399 0.138964 5.17273
-2.6318 0.120702 5.1164
-2.6913 0.13397 5.21704
0.912429 -0.097135 4.23787
-----------------------------------------已知的2D points如下-----------------------------------------
u v
394 253
412.8 176.4
216 181
652.32 311.04
652.8 312
409.536 174.528
324.864 207.36
653.184 311.04
405.596 251.32
406.094 176.173
591 229
293 223
293 223
591.6 229.2
321.6 218.4
292.8 223.2
293.76 221.76
292.032 222.912
592.22 228.925
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <chrono>
#include "Eigen/Core"
using namespace std;
using namespace Eigen;
double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;
struct BA_3d_2d
{
BA_3d_2d(double x, double y) : observed_u(x), observed_v(y) {}
template<typename T>
bool operator()(const T* const tcw, const T* const qcw, const T* Pw, T* residuals) const
{
T pc[3];
ceres::QuaternionRotatePoint(qcw, Pw, pc);
pc[0] += tcw[0]; pc[1] += tcw[1]; pc[2] += tcw[2];
T x = pc[0] / pc[2];
T y = pc[1] / pc[2];
T u = fx * x + cx;
T v = fy * y + cy;
// 跟现有观测形成残差
residuals[0] = u - T(observed_u);
residuals[1] = v - T(observed_v);
return true;
}
const double observed_u, observed_v;
};
template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
{
Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
ans << typename Derived::Scalar(0), -q(2), q(1),
q(2), typename Derived::Scalar(0), -q(0),
-q(1), q(0), typename Derived::Scalar(0);
return ans;
}
template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
typedef typename Derived::Scalar Scalar_t;
Eigen::Quaternion<Scalar_t> dq;
Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
half_theta /= static_cast<Scalar_t>(2.0);
dq.w() = static_cast<Scalar_t>(1.0);
dq.x() = half_theta.x();
dq.y() = half_theta.y();
dq.z() = half_theta.z();
return dq;
}
class ProjectionFactor : public ceres::SizedCostFunction<2, 3, 4, 3>
{
public:
ProjectionFactor(const Eigen::Vector2d &_pix):pix(_pix){}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
void check(double **parameters);
Eigen::Vector2d pix;
};
bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Vector3d Pcw(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qcw(parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3]);
Eigen::Vector3d Point_w(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Vector3d Point_c = Qcw * Point_w + Pcw;
double x = Point_c[0] / Point_c[2];
double y = Point_c[1] / Point_c[2];
double u = fx * x + cx;
double v = fy * y + cy;
residuals[0] = -(u - pix[0]);
residuals[1] = -(v - pix[1]);
double z_c = 1.0/Point_c[2];
assert(z_c >= 1e-3);
Eigen::Matrix<double, 2, 3> J_e2Point_c;
J_e2Point_c << -fx*z_c, 0.0, fx*Point_c[0]*z_c*z_c,
0.0, -fy*z_c, fy*Point_c[1]*z_c*z_c;
if (jacobians)
{
Eigen::Matrix3d Rcw = Qcw.matrix();
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian_pose_t(jacobians[0]);
Eigen::Matrix<double, 3, 3> J_Point_c2tcw;
J_Point_c2tcw.setIdentity();
jacobian_pose_t = J_e2Point_c * J_Point_c2tcw;
}
if (jacobians[1])
{
/* https://www.cnblogs.com/JingeTU/p/11707557.html*/
Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> jacobian_pose_R(jacobians[1]);
Eigen::Matrix<double, 3, 3> J_Point_c2Rcw;
J_Point_c2Rcw = -(skewSymmetric(Rcw*Point_w));
jacobian_pose_R.leftCols(3) = J_e2Point_c * J_Point_c2Rcw;
jacobian_pose_R.rightCols(1).setZero();
Eigen::Matrix<double, 4, 3> I;
I.topRows<3>().setIdentity();
I.bottomRows<1>().setZero();
Eigen::Matrix<double, 2, 3> J_new;
J_new = jacobian_pose_R * I;
Eigen::Matrix<double, 4, 3> J43;
J43 << -Qcw.x(), -Qcw.y(), -Qcw.z(),
Qcw.w(), Qcw.z(), -Qcw.y(),
-Qcw.z(), Qcw.w(), Qcw.x(),
Qcw.y(), -Qcw.x(), Qcw.w();
jacobian_pose_R = J_new* J43.transpose() * 2;
}
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> jacobian_point(jacobians[2]);
Eigen::Matrix<double, 3, 3> J_Point_c2pw;
J_Point_c2pw = Rcw;
jacobian_point = J_e2Point_c * J_Point_c2pw;
}
}
return true;
}
int main(int argc, char **argv)
{
/* 认为图像1403637189138319104为第一帧，也就是世界坐标系，图像1403637188088318976为下一帧，并求图像1403637188088318976在世界坐标系下的位姿，以及三角化之后的3D点的世界坐标*/
/**
* Ceres自动求导的基本原理：使用dual数，即对偶数，并结合jet.h中实现的两个dual数常见的算数运算法则，包括加减乘除，指数等。借助c++模板实现的dual数的运算
* */
/**
* 通过计算优化时间，可以发现，手动求导的时间少于自动求导，即：相同条件下，手动求导的计算效率更高
* */
ifstream f1, f2;
Matrix3d Rcw;
Rcw << 0.979523, -0.0866216, 0.181747,
0.0960453, 0.994418, -0.0436899,
-0.176948, 0.0602511, 0.982374;
Vector3d tcw;
tcw << 0.655447, 0.0119566, 0.755146;
Quaterniond qcw(Rcw);
double Pose_t[3], Pose_R[4];
Pose_t[0] = tcw.x();
Pose_t[1] = tcw.y();
Pose_t[2] = tcw.z();
Pose_R[0] = qcw.w();
Pose_R[1] = qcw.x();
Pose_R[2] = qcw.y();
Pose_R[3] = qcw.z();
f1.open("/home/biao/real/homework/task6/6_2/2Dpoints.txt");
f2.open("/home/biao/real/homework/task6/6_2/3Dpoints.txt");
vector<Vector2d> v_pix;
vector<Vector3d> v_landmark;
while(!f1.eof())
{
string s;
getline(f1, s);
if (!s.empty())
{
stringstream ss;
ss << s;
Vector2d pix;
ss >> pix[0];
ss >> pix[1];
v_pix.push_back(pix);
}
}
double point_w_3d[v_pix.size()][3];
int i_ = 0;
while(!f2.eof())
{
string s;
getline(f2, s);
if (!s.empty())
{
stringstream ss;
ss << s;
Vector3d landmark;
ss >> landmark[0];
ss >> landmark[1];
ss >> landmark[2];
point_w_3d[i_][0] = landmark[0];
point_w_3d[i_][1] = landmark[1];
point_w_3d[i_][2] = landmark[2];
v_landmark.push_back(landmark);
}
i_++;
}
ceres::Problem problem;
ceres::LossFunction *loss_function;
loss_function = new ceres::CauchyLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
problem.AddParameterBlock(Pose_t, 3);
problem.AddParameterBlock(Pose_R, 4, local_parameterization);
for(int i = 0; i < v_pix.size(); ++i)
{
auto *f_manu = new ProjectionFactor(v_pix[i]);
auto *f_auto = new ceres::AutoDiffCostFunction<BA_3d_2d, 2, 3, 4, 3>(new BA_3d_2d(v_pix[i][0], v_pix[i][1]));
problem.AddResidualBlock
(
// f_auto, /** 选择自动求导*/
f_manu, /** 选择手动求导*/
loss_function,
Pose_t,
Pose_R,
point_w_3d[i]
);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
options.gradient_check_relative_precision = 1e-5;
options.check_gradients = true;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);//duration为持续时间
cout << "手动求导用时：" << time_used.count()*1000 << " 毫秒" << endl;
ofstream o; //将优化后的3D点写入文件
o.open("/home/biao/real/homework/task6/6_2/3Dpoints_optimize.txt", ios::out);
for(int i = 0; i < v_pix.size(); ++i)
{
o << point_w_3d[i][0]; o << " ";
o << point_w_3d[i][1]; o << " ";
o << point_w_3d[i][2];
o << endl;
}
Quaterniond qcw_optimize(Pose_R[0],Pose_R[1],Pose_R[2],Pose_R[3]);
Matrix3d Rcw_optimize = qcw_optimize.matrix();
Vector3d tcw_optimize;
tcw_optimize << Pose_t[0], Pose_t[1], Pose_t[2];
cout << "--------------------优化前的Rcw和tcw--------------------" << endl;
cout << "Rcw = " << endl << Rcw << endl;
cout << "tcw = " << endl << tcw.transpose() << endl;
cout << "--------------------优化后的Rcw和tcw--------------------" << endl;
cout << "Rcw_optimize = " << endl << Rcw_optimize << endl;
cout << "tcw_optimize = " << endl << tcw_optimize.transpose() << endl;
std::cout << summary.FullReport() << "\n";
return 0;
}
**重点看上面的 **if (jacobians[1])中对旋转部分的导数，并参考下面两个链接，一定要看！！！
526
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
