http://www.ceres-solver.org/
一.仅优化位姿
构造类和代价函数：
// 代价函数的计算模型
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);
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*K.at<double>(0,0)+K.at<double>(0,2);
T v_= yp*K.at<double>(1,1)+K.at<double>(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;
};
其中，形参uv是坐标点对，xyz是路标点，由于仅仅优化位姿，我们假设路标点确定，像素点对由特征匹配得到，路标为世界坐标，也即第一帧相机坐标，AngleAxisRotatePoint在头文件rotation.h中，它根据相机位姿（旋转向量和平移向量表示，构成的6维数组，不对内参焦距进行优化，不考虑相机畸变），路标点（三维数组），计算得到RP，结合平移向量得到相机坐标，进而得到投影。
位姿初值：
double camera[6]={0,1,2,0,0,0};
最小二乘问题的构建：
ceres::Problem problem;
for (int i = 0; i < pts_2d.size(); ++i)
{
ceres::CostFunction* cost_function =
PnPCeres::Create(pts_2d[i],pts_3d[i]);
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
camera);
}
配置求解器与结构输出：
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
Mat R_vec = (Mat_<double>(3,1) << camera[0],camera[1],camera[2]);//数组转cv向量
Mat R_cvest;
Rodrigues(R_vec,R_cvest);//罗德里格斯公式，旋转向量转旋转矩阵
cout<<"R_cvest="<<R_cvest<<endl;
Eigen::Matrix3d R_est;
cv2eigen(R_cvest,R_est);//cv矩阵转eigen矩阵
cout<<"R_est="<<R_est<<endl;
Eigen::Vector3d t_est(camera[3],camera[4],camera[5]);
cout<<"t_est="<<t_est<<endl;
Eigen::Isometry3d T(R_est);//构造变换矩阵与输出
T.pretranslate(t_est);
cout<<T.matrix()<<endl;
return 0;
优化结果：
/home/luoyongheng/study_slam/ch06/ceres_curve_fitting/cmake-build-debug/ICP_G2O 1.png 2.png 1_depth.png 2_depth.png
[ INFO:0] Initialize OpenCL runtime...
-- Max dist : 95.000000
-- Min dist : 7.000000
一共找到了79组匹配点
3d-2d pairs: 76
----------------optional berore--------------------
R=
[0.9978662026232452, -0.05167241669574823, 0.03991244185821538;
0.0505958920800001, 0.9983397626650758, 0.02752769489929838;
-0.04126860025362437, -0.02544955077522656, 0.9988239199170421]
t=
[-0.6361298163857759;
-0.03753650998182134;
0.3069292389205321]
r=
[-0.02651058658732764;
0.04062417777204908;
0.0511765536395706]
----------------optional after--------------------
R_cvest=[0.9978660137045686, -0.05166958437335226, 0.0399208309537603;
0.05059260904994334, 0.9983397718877706, 0.02753339384445446;
-0.04127719228427807, -0.02545493896593123, 0.9988234275783521]
R_est= 0.997866 -0.0516696 0.0399208
0.0505926 0.99834 0.0275334
-0.0412772 -0.0254549 0.998823
t_est= -0.636199
-0.0375824
0.306929
0.997866 -0.0516696 0.0399208 -0.636199
0.0505926 0.99834 0.0275334 -0.0375824
-0.0412772 -0.0254549 0.998823 0.306929
0 0 0 1
完整代码：
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <chrono>
#include "rotation.h" // ch10 tool
using namespace std;
using namespace cv;
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
// 代价函数的计算模型 仅优化位姿
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);
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*K.at<double>(0,0)+K.at<double>(0,2);
T v_= yp*K.at<double>(1,1)+K.at<double>(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;
};
//其中，形参uv是坐标点对，xyz是路标点，由于仅仅优化位姿，我们假设路标点确定，像素点对由特征匹配得到，路标为世界坐标，
//也即第一帧相机坐标，AngleAxisRotatePoint在头文件rotation.h中，它根据相机位姿（旋转向量和平移向量表示，构成的6维数组，
//不对内参焦距进行优化，不考虑相机畸变），路标点（三维数组），计算得到RP，结合平移向量得到相机坐标，进而得到投影。
void find_feature_matches (
const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches );
// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
int main ( int argc, char** argv ) {
double camera[6]={0,1,2,0,0,0}; //位姿初值： 第二部 PNP 给值,再ceres
if (argc != 5) {
cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
return 1;
}
//-- 读取图像
Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "组匹配点" << endl;
// 建立3D点
Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for (DMatch m:matches) {
ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}
// double pt[3*pts_3d.size()];
// int i=0;
// for(auto p:pts_3d)
// {
// pt[i]=p.x;
// pt[++i]=p.y;
// pt[++i]=p.z;
// ++i;
// }
cout << "3d-2d pairs: " << pts_3d.size() << endl;
Mat r, t;
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
cout << "R=" << endl << R << endl;
cout << "t=" << endl << t << endl;
cout << "r=" << endl << r << endl;
ceres::Problem problem;//最小二乘问题的构建：
for (int i = 0; i < pts_2d.size(); ++i)
{
ceres::CostFunction* cost_function =
PnPCeres::Create(pts_2d[i],pts_3d[i]);
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
camera);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
Mat R_vec = (Mat_<double>(3,1) << camera[0],camera[1],camera[2]);//数组转cv向量
Mat R_cvest;
Rodrigues(R_vec,R_cvest);//罗德里格斯公式，旋转向量转旋转矩阵
cout<<"R_cvest="<<R_cvest<<endl;
Eigen::Matrix3d R_est;
cv2eigen(R_cvest,R_est);//cv矩阵转eigen矩阵
cout<<"R_est="<<R_est<<endl;
Eigen::Vector3d t_est(camera[3],camera[4],camera[5]);
cout<<"t_est="<<t_est<<endl;
Eigen::Isometry3d T(R_est);//构造变换矩阵与输出
T.pretranslate(t_est);
cout<<T.matrix()<<endl;
return 0;
}
void find_feature_matches ( const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches )
{
//-- 初始化
Mat descriptors_1, descriptors_2;
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
// use this if you are in OpenCV2
// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect ( img_1,keypoints_1 );
detector->detect ( img_2,keypoints_2 );
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute ( img_1, keypoints_1, descriptors_1 );
descriptor->compute ( img_2, keypoints_2, descriptors_2 );
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match ( descriptors_1, descriptors_2, match );
//-- 第四步:匹配点对筛选
double min_dist=10000, max_dist=0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for ( int i = 0; i < descriptors_1.rows; i++ )
{
double dist = match[i].distance;
if ( dist < min_dist ) min_dist = dist;
if ( dist > max_dist ) max_dist = dist;
}
printf ( "-- Max dist : %f \n", max_dist );
printf ( "-- Min dist : %f \n", min_dist );
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for ( int i = 0; i < descriptors_1.rows; i++ )
{
if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
{
matches.push_back ( match[i] );
}
}
}
Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
return Point2d
(
( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
);
}注意：
-
这里的R不是旋转矩阵，也不是四元数表示的，而是用旋转向量表示的。
通过函数AngleAxisRotatePoint(cere_r,p_1,p_2)可以对3D点进行旋转。相当于用旋转矩阵去左乘。 -
这里相机内参没有进行优化，而是直接写入，要一起优化可以稍加修改即可。这里优化的只有相机外参。
-
这里因为有模板，因此要将Point3f类型的_p1转为模板类型p_1，这样才可以在模板类型中的元素进行运算。否则会报错。
-
还有遇到了奇葩的问题，如果将观测变量
_p1由类型Point3f改为double*,则优化结果完全错误。调试发现，传入的观测_p1始终是第一次的值，后面没有再改变。猜测可能是数组必须按地址传递造成的。我将其类型改为自己写的struct类型则正确，初步验证了我的猜想，但是ceres内部怎么写的造成这样，还不太清楚。 -
传入的观测是第一帧坐标系下的3D点坐标，和第二帧图像坐标系下的二维点。因为类型不统一，而又必须要用模板参数，因此这里统一类型，修改为
double*。
笔记
1 逆深度->XYZ
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );2 3d_2d
传入的观测是第一帧坐标系下的3D点坐标，和第二帧图像坐标系下的二维点。因为类型不统一，而又必须要用模板参数，因此这里统一类型，修改为double*。
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for (DMatch m:matches) {
ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}3 opencv中DMatch解释
queryIdx : 查询点的索引（当前要寻找匹配结果的点在它所在图片上的索引）.
trainIdx : 被查询到的点的索引（存储库中的点的在存储库上的索引）
可以看出 queryIdx是按顺序增大的， 相应的trainIdx是不断跳变的，且imgIdx一直是0.
可以从matches[0]看出，这里的queryIdx是指第0张图的第0个特征点匹配到了第1张图的第6个特征点（距离最近），同时第0张图的第0个特征点匹配到了第1张图的第128个特征点（距离第2近）。
于是 queryIdx 是你想要为“它”找到匹配结果的“它”的索引。 trainIdx是 “它”的匹配结果的索引。
由于imgIdx一直是0，只能说在这里可能没什么用。有些文章说 imgIdx指的是第0张图，有的说是第1张图的意思，从这里感觉更应该是指第0张图。。
原文链接：https://blog.csdn.net/wphkadn/article/details/85805105
二.添加路标点优化
添加路标点优化，位姿和路标结果相差太大，预计数据量不够多，约束不足？？？哪位知道，请赐教！
1139
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
