https://lsy.cowtransfer.com/s/e49d4abb51d944
您好，文件已下载并上传完毕，请用浏览器打开链接下载
ttps://github.com/opencv/opencv
//
// Created by caoqi on 2018/9/5.
//
#include <iostream>
#include "math/vector.h"
class Camera{
public:
// constructor
Camera(){
// 采用归一化坐标，不考虑图像尺寸
c_[0]=c_[1] = 0.0;
}
// 相机投影过程
math::Vec2d projection(math::Vec3d const & p3d){
math::Vec2d p;
/** TODO HERE
*
*/
return p;
/** Reference
// 世界坐标系到相机坐标系
double xc = R_[0] * p3d[0] + R_[1] * p3d[1] + R_[2]* p3d[2] + t_[0];
double yc = R_[3] * p3d[0] + R_[4] * p3d[1] + R_[5]* p3d[2] + t_[1];
double zc = R_[6] * p3d[0] + R_[7] * p3d[1] + R_[8]* p3d[2] + t_[2];
// 相机坐标系到像平面
double x = xc/zc;
double y = yc/zc;
// 径向畸变过程
double r2 = x*x + y*y;
double distort_ratio = 1+ dist_[0]* r2+ dist_[1]*r2*r2;
// 图像坐标系到屏幕坐标系
math::Vec2d p;
p[0] = f_* distort_ratio*x + c_[0];
p[1] = f_* distort_ratio*y + c_[1];
return p;
**/
}
// 相机在世界坐标中的位置 -R^T*t
math::Vec3d pos_in_world(){
math::Vec3d pos;
pos[0] = R_[0]* t_[0] + R_[3]* t_[1] + R_[6]* t_[2];
pos[1] = R_[1]* t_[0] + R_[4]* t_[1] + R_[7]* t_[2];
pos[2] = R_[2]* t_[0] + R_[5]* t_[1] + R_[8]* t_[2];
return -pos;
}
// 相机在世界坐标中的方向
math::Vec3d dir_in_world(){
math::Vec3d dir (R_[6], R_[7],R_[8]);
return dir;
}
public:
// 焦距f
double f_;
// 径向畸变系数k1, k2
double dist_[2];
// 中心点坐标u0, v0
double c_[2];
// 旋转矩阵
/*
* [ R_[0], R_[1], R_[2] ]
* [ R_[3], R_[4], R_[5] ]
* [ R_[6], R_[7], R_[8] ]
*/
double R_[9];
// 平移向量
double t_[3];
};
int main(int argc, char* argv[]){
Camera cam;
//焦距
cam.f_ = 0.920227;
// 径向畸变系数
cam.dist_[0] = -0.106599; cam.dist_[1] = 0.104385;
// 平移向量
cam.t_[0] = 0.0814358; cam.t_[1] = 0.937498; cam.t_[2] = -0.0887441;
// 旋转矩阵
cam.R_[0] = 0.999796 ; cam.R_[1] = -0.0127375; cam.R_[2] = 0.0156807;
cam.R_[3] = 0.0128557; cam.R_[4] = 0.999894 ; cam.R_[5] = -0.0073718;
cam.R_[6] = -0.0155846; cam.R_[7] = 0.00757181; cam.R_[8] = 0.999854;
// 三维点坐标
math::Vec3d p3d ={1.36939, -1.17123, 7.04869};
/*计算相机的投影点*/
math::Vec2d p2d = cam.projection(p3d);
std::cout<<"projection coord:\n "<<p2d<<std::endl;
std::cout<<"result should be:\n 0.208188 -0.035398\n\n";
/*计算相机在世界坐标系中的位置*/
math::Vec3d pos = cam.pos_in_world();
std::cout<<"cam position in world is:\n "<< pos<<std::endl;
std::cout<<"result should be: \n -0.0948544 -0.935689 0.0943652\n\n";
/*计算相机在世界坐标系中的方向*/
math::Vec3d dir = cam.dir_in_world();
std::cout<<"cam direction in world is:\n "<<dir<<std::endl;
std::cout<<"result should be: \n -0.0155846 0.00757181 0.999854\n";
}
//Created by sway on 2018/8/29.
/* 测试8点法求取基础矩阵F
*
* [直接线性变换法]
* 双目视觉中相机之间存在对极约束
*
* p2'Fp1=0,
*
* 其中p1, p2 为来自两个视角的匹配对的归一化坐标，并表示成齐次坐标形式，
* 即p1=[x1, y1, z1]', p2=[x2, y2, z2],将p1, p2的表达形式带入到
* 上式中，可以得到如下表达形式
*
* [x2] [f11, f12, f13] [x1, y1, z1]
* [y2] [f21, f22, f23] = 0
* [z2] [f31, f32, f33]
*
* 进一步可以得到
* x1*x2*f11 + x2*y1*f12 + x2*f13 + x1*y2*f21 + y1*y2*f22 + y2*f23 + x1*f31 + y1*f32 + f33=0
*
* 写成向量形式
* [x1*x2, x2*y1,x2, x1*y2, y1*y2, y2, x1, y1, 1]*f = 0,
* 其中f=[f11, f12, f13, f21, f22, f23, f31, f32, f33]'
*
* 由于F无法确定尺度(up to scale, 回想一下三维重建是无法确定场景真实尺度的)，因此F秩为8，
* 这意味着至少需要8对匹配对才能求的f的解。当刚好有8对点时，称为8点法。当匹配对大于8时需要用最小二乘法进行求解
*
* [x11*x12, x12*y11,x12, x11*y12, y11*y12, y12, x11, y11, 1]
* [x21*x22, x22*y21,x22, x21*y22, y21*y22, y22, x21, y21, 1]
* [x31*x32, x32*y31,x32, x31*y32, y31*y32, y32, x31, y31, 1]
* A=[x41*x42, x42*y41,x42, x41*y42, y41*y42, y42, x41, y41, 1]
* [x51*x52, x52*y51,x52, x51*y52, y51*y52, y52, x51, y51, 1]
* [x61*x62, x62*y61,x62, x61*y62, y61*y62, y62, x61, y61, 1]
* [x71*x72, x72*y71,x72, x71*y72, y71*y72, y72, x71, y71, 1]
* [x81*x82, x82*y81,x82, x81*y22, y81*y82, y82, x81, y81, 1]
*
*现在任务变成了求解线性方程
* Af = 0
*（该方程与min||Af||, subject to ||f||=1 等价)
*通常的解法是对A进行SVD分解，取最小奇异值对应的奇异向量作为f分解
*
*本项目中对矩阵A的svd分解并获取其最小奇异值对应的奇异向量的代码为
* math::Matrix<double, 9, 9> V;
* math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &V);
* math::Vector<double, 9> f = V.col(8);
*
*
*[奇异性约束]
* 基础矩阵F的一个重要的性质是F是奇异的，秩为2，因此有一个奇异值为0。通过上述直接线性法求得
* 矩阵不具有奇异性约束。常用的方法是将求得得矩阵投影到满足奇异约束得空间中。
* 具体地，对F进行奇异值分解
* F = USV'
* 其中S是对角矩阵，S=diag[sigma1, sigma2, sigma3]
* 将sigma3设置为0，并重构F
* [sigma1, 0, ,0]
* F = U [ 0 , sigma2 ,0] V'
* [ 0 , 0 ,0]
*/
#include <math/matrix_svd.h>
#include "math/matrix.h"
#include "math/vector.h"
typedef math::Matrix<double, 3, 3> FundamentalMatrix;
FundamentalMatrix fundamental_8_point (math::Matrix<double, 3, 8> const& points1
, math::Matrix<double, 3, 8> const& points2
){
FundamentalMatrix F;
/** TODO HERE
* Coding Here!!
*/
return F;
#if 0
/* direct linear transform */
math::Matrix<double, 8, 9> A;
for(int i=0; i<8; i++)
{
math::Vec3d p1 = points1.col(i);
math::Vec3d p2 = points2.col(i);
A(i, 0) = p1[0]*p2[0];
A(i, 1) = p1[1]*p2[0];
A(i, 2) = p2[0];
A(i, 3) = p1[0]*p2[1];
A(i, 4) = p1[1]*p2[1];
A(i, 5) = p2[1];
A(i, 6) = p1[0];
A(i, 7) = p1[1];
A(i, 8) = 1.0;
}
math::Matrix<double, 9, 9> vv;
math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &vv);
math::Vector<double, 9> f = vv.col(8);
FundamentalMatrix F;
F(0,0) = f[0]; F(0,1) = f[1]; F(0,2) = f[2];
F(1,0) = f[3]; F(1,1) = f[4]; F(1,2) = f[5];
F(2,0) = f[6]; F(2,1) = f[7]; F(2,2) = f[8];
/* singularity constraint */
math::Matrix<double, 3, 3> U, S, V;
math::matrix_svd(F, &U, &S, &V);
S(2,2)=0;
F = U*S*V.transpose();
return F;
#endif
}
int main(int argc, char*argv[])
{
// 第一幅图像中的对应点
math::Matrix<double, 3, 8> pset1;
pset1(0, 0) = 0.180123 ; pset1(1, 0)= -0.156584; pset1(2, 0)=1.0;
pset1(0, 1) = 0.291429 ; pset1(1, 1)= 0.137662 ; pset1(2, 1)=1.0;
pset1(0, 2) = -0.170373; pset1(1, 2)= 0.0779329; pset1(2, 2)=1.0;
pset1(0, 3) = 0.235952 ; pset1(1, 3)= -0.164956; pset1(2, 3)=1.0;
pset1(0, 4) = 0.142122 ; pset1(1, 4)= -0.216048; pset1(2, 4)=1.0;
pset1(0, 5) = -0.463158; pset1(1, 5)= -0.132632; pset1(2, 5)=1.0;
pset1(0, 6) = 0.0801864; pset1(1, 6)= 0.0236417; pset1(2, 6)=1.0;
pset1(0, 7) = -0.179068; pset1(1, 7)= 0.0837119; pset1(2, 7)=1.0;
//第二幅图像中的对应
math::Matrix<double, 3, 8> pset2;
pset2(0, 0) = 0.208264 ; pset2(1, 0)= -0.035405 ; pset2(2, 0) = 1.0;
pset2(0, 1) = 0.314848 ; pset2(1, 1)= 0.267849 ; pset2(2, 1) = 1.0;
pset2(0, 2) = -0.144499; pset2(1, 2)= 0.190208 ; pset2(2, 2) = 1.0;
pset2(0, 3) = 0.264461 ; pset2(1, 3)= -0.0404422; pset2(2, 3) = 1.0;
pset2(0, 4) = 0.171033 ; pset2(1, 4)= -0.0961747; pset2(2, 4) = 1.0;
pset2(0, 5) = -0.427861; pset2(1, 5)= 0.00896567; pset2(2, 5) = 1.0;
pset2(0, 6) = 0.105406 ; pset2(1, 6)= 0.140966 ; pset2(2, 6) = 1.0;
pset2(0, 7) = -0.15257; pset2(1, 7)= 0.19645 ; pset2(2, 7) = 1.0;
FundamentalMatrix F = fundamental_8_point(pset1, pset2);
std::cout<<"Fundamental matrix after singularity constraint is:\n "<<F<<std::endl;
std::cout<<"Result should be: \n"<<"-0.0315082 -0.63238 0.16121\n"
<<"0.653176 -0.0405703 0.21148\n"
<<"-0.248026 -0.194965 -0.0234573\n" <<std::endl;
return 0;
}
/*
* Copyright (C) 2015, Simon Fuhrmann
* TU Darmstadt - Graphics, Capture and Massively Parallel Computing
* All rights reserved.
*
* This software may be modified and distributed under the terms
* of the BSD 3-Clause license. See the LICENSE.txt file for details.
*/
#include <cassert> //add
#include <iostream>
#include <fstream>
#include <sstream>
#include <set>
#include <util/system.h>
#include <sfm/ransac_fundamental.h>
#include "math/functions.h"
#include "sfm/fundamental.h"
#include "sfm/correspondence.h"
#include "math/matrix_svd.h"
typedef math::Matrix<double, 3, 3> FundamentalMatrix;
/**
* \description 用于RANSAC采样成功所需要的采样次数
* @param p -- 内点的概率
* @param K --拟合模型需要的样本个数，对应基础矩阵num_samples=8
* @param z -- 预期的采样成功的概率
* log(1-z)
* 需要的采样次数 M = -----------
* log(1-p^K)
* Example: For p = 50%, z = 99%, n = 8: M = log(0.001) / log(0.99609) = 1176.
* 需要采样1176次从而保证RANSAC的成功率不低于0.99.
* @return
*/
int calc_ransac_iterations (double p,
int K,
double z = 0.99){
//add by myself
double iteration_time=std::log(1.0-z)
/std::log(1.0-std::pow(p,K));
return int(iteration_time );
/** TODO HERE
* Coding here**/
return 0;
/** Reference
double prob_all_good = math::fastpow(p, K);
double num_iterations = std::log(1.0 - z)
/ std::log(1.0 - prob_all_good);
return static_cast<int>(math::round(num_iterations));
*/
}
/**
* \description 给定基础矩阵和一对匹配点，计算匹配点的sampson 距离，用于判断匹配点是否是内点,
* 计算公式如下：
* SD = (x'Fx)^2 / ( (Fx)_1^2 + (Fx)_2^2 + (x'F)_1^2 + (x'F)_2^2 )
* @param F-- 基础矩阵
* @param m-- 匹配对
* @return
*/
double calc_sampson_distance (FundamentalMatrix const& F, sfm::Correspondence2D2D const& m) {
double p2_F_p1 = 0.0;
p2_F_p1 += m.p2[0] * (m.p1[0] * F[0] + m.p1[1] * F[1] + F[2]);
p2_F_p1 += m.p2[1] * (m.p1[0] * F[3] + m.p1[1] * F[4] + F[5]);
p2_F_p1 += 1.0 * (m.p1[0] * F[6] + m.p1[1] * F[7] + F[8]);
p2_F_p1 *= p2_F_p1;
double sum = 0.0;
sum += math::fastpow(m.p1[0] * F[0] + m.p1[1] * F[1] + F[2], 2);
sum += math::fastpow(m.p1[0] * F[3] + m.p1[1] * F[4] + F[5], 2);
sum += math::fastpow(m.p2[0] * F[0] + m.p2[1] * F[3] + F[6], 2);
sum += math::fastpow(m.p2[0] * F[1] + m.p2[1] * F[4] + F[7], 2);
return p2_F_p1 / sum;
}
/**
* \description 8点发估计相机基础矩阵
* @param pset1 -- 第一个视角的特征点
* @param pset2 -- 第二个视角的特征点
* @return 估计的基础矩阵
*/
void calc_fundamental_8_point (math::Matrix<double, 3, 8> const& pset1
, math::Matrix<double, 3, 8> const& pset2
,FundamentalMatrix &F
){
/* direct linear transform */
math::Matrix<double, 8, 9> A;
for(int i=0; i<8; i++)
{
math::Vec3d p1 = pset1.col(i);
math::Vec3d p2 = pset2.col(i);
A(i, 0) = p1[0]*p2[0];
A(i, 1) = p1[1]*p2[0];
A(i, 2) = p2[0];
A(i, 3) = p1[0]*p2[1];
A(i, 4) = p1[1]*p2[1];
A(i, 5) = p2[1];
A(i, 6) = p1[0];
A(i, 7) = p1[1];
A(i, 8) = 1.0;
}
math::Matrix<double, 9, 9> vv;
math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &vv);
math::Vector<double, 9> f = vv.col(8);
F(0,0) = f[0]; F(0,1) = f[1]; F(0,2) = f[2];
F(1,0) = f[3]; F(1,1) = f[4]; F(1,2) = f[5];
F(2,0) = f[6]; F(2,1) = f[7]; F(2,2) = f[8];
/* singularity constraint */
math::Matrix<double, 3, 3> U, S, V;
math::matrix_svd(F, &U, &S, &V);
S(2,2)=0;
F = U*S*V.transpose();
}
/**
* \description 利用最小二乘法计算基础矩阵
* @param matches--输入的匹配对 大于8对
* @param F --基础矩阵
*/
void calc_fundamental_least_squares(sfm::Correspondences2D2D const & matches, FundamentalMatrix&F){
if (matches.size() < 8)
throw std::invalid_argument("At least 8 points required");
/* Create Nx9 matrix A. Each correspondence creates on row in A. */
std::vector<double> A(matches.size() * 9);
for (std::size_t i = 0; i < matches.size(); ++i)
{
sfm::Correspondence2D2D const& p = matches[i];
A[i * 9 + 0] = p.p2[0] * p.p1[0];
A[i * 9 + 1] = p.p2[0] * p.p1[1];
A[i * 9 + 2] = p.p2[0] * 1.0;
A[i * 9 + 3] = p.p2[1] * p.p1[0];
A[i * 9 + 4] = p.p2[1] * p.p1[1];
A[i * 9 + 5] = p.p2[1] * 1.0;
A[i * 9 + 6] = 1.0 * p.p1[0];
A[i * 9 + 7] = 1.0 * p.p1[1];
A[i * 9 + 8] = 1.0 * 1.0;
}
/* Compute fundamental matrix using SVD. */
std::vector<double> vv(9 * 9);
math::matrix_svd<double>(&A[0], matches.size(), 9, nullptr, nullptr, &vv[0]);
/* Use last column of V as solution. */
for (int i = 0; i < 9; ++i)
F[i] = vv[i * 9 + 8];
/* singularity constraint */
math::Matrix<double, 3, 3> U, S, V;
math::matrix_svd(F, &U, &S, &V);
S(2,2)=0;
F = U*S*V.transpose();
}
/**
* \description 给定匹配对和基础矩阵，计算内点的个数
* @param matches
* @param F
* @return
*/
std::vector<int> find_inliers(sfm::Correspondences2D2D const & matches
,FundamentalMatrix const & F, const double & thresh){
const double squared_thresh = thresh* thresh;
std::vector<int> inliers;
for(int i=0; i< matches.size(); i++){
if(calc_sampson_distance(F,matches[i])<squared_thresh){
inliers.push_back(i);
}
}
/**
* TODO HERE
*
* Coding here **/
/** Reference
for(int i=0; i< matches.size(); i++){
double error = calc_sampson_distance(F, matches[i]);
if(error< squared_thresh){
inliers.push_back(i);
}
}
**/
return inliers;
}
int main(int argc, char *argv[]){
/** 加载归一化后的匹配对 */
sfm::Correspondences2D2D corr_all;
std::ifstream in("./examples/task1/correspondences.txt");
assert(in.is_open());
std::string line, word;
int n_line = 0;
while(getline(in, line)){
std::stringstream stream(line);
if(n_line==0){
int n_corrs = 0;
stream>> n_corrs;
corr_all.resize(n_corrs);
n_line ++;
continue;
}
if(n_line>0){
stream>>corr_all[n_line-1].p1[0]>>corr_all[n_line-1].p1[1]
>>corr_all[n_line-1].p2[0]>>corr_all[n_line-1].p2[1];
}
n_line++;
}
/* 计算采用次数 */
const float inlier_ratio =0.5;
const int n_samples=8;
int n_iterations = calc_ransac_iterations(inlier_ratio, n_samples);
// 用于判读匹配对是否为内点
const double inlier_thresh = 0.0015;
// ransac 最终估计的内点
std::vector<int> best_inliers;
std::cout << "RANSAC-F: Running for " << n_iterations
<< " iterations, threshold " << inlier_thresh
<< "..." << std::endl;
for(int i=0; i<n_iterations; i++){
/* 1.0 随机找到8对不重复的匹配点 */
std::set<int> indices;
while(indices.size()<8){
indices.insert(util::system::rand_int() % corr_all.size());
}
math::Matrix<double, 3, 8> pset1, pset2;
std::set<int>::const_iterator iter = indices.cbegin();
for(int j=0; j<8; j++, iter++){
sfm::Correspondence2D2D const & match = corr_all[*iter];
pset1(0, j) = match.p1[0];
pset1(1, j) = match.p1[1];
pset1(2, j) = 1.0;
pset2(0, j) = match.p2[0];
pset2(1, j) = match.p2[1];
pset2(2, j) = 1.0;
}
/*2.0 8点法估计相机基础矩阵*/
FundamentalMatrix F;
calc_fundamental_8_point(pset1, pset2,F);
/*3.0 统计所有的内点个数*/
std::vector<int> inlier_indices = find_inliers(corr_all, F, inlier_thresh);
if(inlier_indices.size()> best_inliers.size()){
// std::cout << "RANSAC-F: Iteration " << i
// << ", inliers " << inlier_indices.size() << " ("
// << (100.0 * inlier_indices.size() / corr_all.size())
// << "%)" << std::endl;
best_inliers.swap(inlier_indices);
}
}
sfm::Correspondences2D2D corr_f;
for(int i=0; i< best_inliers.size(); i++){
corr_f.push_back(corr_all[best_inliers[i]]);
}
/*利用所有的内点进行最小二乘估计*/
FundamentalMatrix F;
calc_fundamental_least_squares(corr_f, F);
std::cout<<"inlier number: "<< best_inliers.size()<<std::endl;
std::cout<<"F\n: "<< F<<std::endl;
std::cout<<"result should be: \n"
<<"inliner number: 272\n"
<<"F: \n"
<<"-0.00961384 -0.0309071 0.703297\n"
<<"0.0448265 -0.00158655 -0.0555796\n"
<<"-0.703477 0.0648517 -0.0117791\n";
return 0;
}
//
// Created by caoqi on 2018/9/5.
//
#include <math/matrix.h>
#include <math/matrix_svd.h>
typedef math::Matrix<double, 3, 3> FundamentalMatrix;
typedef math::Matrix<double, 3, 3> EssentialMatrix;
//用于测试相机姿态的正确性
math::Vec2d p1={0.18012331426143646, -0.15658402442932129};
math::Vec2d p2={0.2082643061876297, -0.035404585301876068};
/*第一个相机的内外参数*/
double f1 = 0.972222208;
/*第二个相机的内外参数*/
double f2 = 0.972222208;
/**
* \description 对匹配点进行三角化得到空间三维点
* @param p1 -- 第一幅图像中的特征点
* @param p2 -- 第二幅图像中的特征点
* @param K1 -- 第一幅图像的内参数矩阵
* @param R1 -- 第一幅图像的旋转矩阵
* @param t1 -- 第一幅图像的平移向量
* @param K2 -- 第二幅图像的内参数矩阵
* @param R2 -- 第二幅图像的旋转矩阵
* @param t2 -- 第二幅图像的平移向量
* @return 三维点
*/
math::Vec3d triangulation(math::Vec2d const & p1
, math::Vec2d const & p2
, math::Matrix3d const &K1
, math::Matrix3d const &R1
, math::Vec3d const & t1
, math::Matrix3d const &K2
, math::Matrix3d const &R2
, math::Vec3d const& t2){
// 构造投影矩阵
math::Matrix<double, 3, 4>P1, P2;
math::Matrix<double, 3, 3> KR1 = K1 * R1;
math::Matrix<double, 3, 1> Kt1(*(K1 * t1));
P1 = KR1.hstack(Kt1);
math::Matrix<double, 3, 3> KR2 = K2 * R2;
math::Matrix<double, 3, 1> Kt2(*(K2 * t2));
P2 = KR2.hstack(Kt2);
std::cout<<"P1: "<<P1<<std::endl;
std::cout<<"P1 for fist pose should be\n"
<<"0.972222 0 0 0\n"
<<"0 0.972222 0 0\n"
<<"0 0 1 0\n";
std::cout<<"P2: "<<P2<<std::endl;
std::cout<<"P2 for fist pose should be\n"
<<" -0.957966 0.165734 -0.00707496 0.0774496\n"
<<"0.164089 0.952816 0.102143 0.967341\n"
<<"0.0250416 0.102292 -0.994439 0.0605768\n";
/* 构造A矩阵 */
math::Matrix<double, 4, 4> A;
// 对A的每一列分别进行赋值
for(int i=0; i<4; i++){
// 第1个点
A(0, i) = p1[0]*P1(2, i) - P1(0, i);
A(1, i) = p1[1]*P1(2, i) - P1(1, i);
// 第2个点
A(2, i) = p2[0]*P2(2, i) - P2(0, i);
A(3, i) = p2[1]*P2(2, i) - P2(1, i);
}
std::cout<<"A: "<<std::endl;
std::cout<<"A for first pose should be:\n"
<<"-0.972222 0 0.180123 0\n"
<<"-0 -0.972222 -0.156584 -0\n"
<<"0.963181 -0.14443 -0.200031 -0.0648336\n"
<<"-0.164975 -0.956437 -0.0669352 -0.969486\n";
math::Matrix<double, 4, 4> V;
math::matrix_svd<double, 4, 4> (A, nullptr, nullptr, &V);
math::Vec3d X;
X[0] = V(0, 3)/V(3, 3);
X[1] = V(1, 3)/V(3, 3);
X[2] = V(2, 3)/V(3, 3);
std::cout<<"X for first pose should be:\n"
<<"3.2043116948585566 -2.7710180887818652 17.195578538234088\n";
return X;
}
/**
* \description 判断相机姿态是否正确，方法是计算三维点在两个相机中的坐标，要求其z坐标大于0，即
* 三维点同时位于两个相机的前方
* @param match
* @param pose1
* @param pose2
* @return
*/
bool is_correct_pose (math::Matrix3d const &R1, math::Vec3d const & t1
,math::Matrix3d const &R2, math::Vec3d const & t2) {
/* 相机内参矩阵 */
math::Matrix3d K1(0.0), K2(0.0);
K1(0, 0) = K1(1, 1) = f1;
K2(0, 0) = K2(1, 1) = f2;
K1(2,2) = 1.0;
K2(2,2) = 1.0;
math::Vec3d p3d = triangulation(p1, p2, K1, R1, t1, K2, R2, t2);
math::Vector<double, 3> x1 = R1 * p3d + t1;
math::Vector<double, 3> x2 = R2 * p3d + t2;
return x1[2] > 0.0f && x2[2] > 0.0f;
}
bool calc_cam_poses(FundamentalMatrix const &F
, const double f1, const double f2
, math::Matrix3d& R
, math::Vec3d & t)
{
/* 相机内参矩阵 */
math::Matrix3d K1(0.0), K2(0.0);
K1(0, 0) = K1(1, 1) = f1; K1(2,2)=1.0;
K2(0, 0) = K2(1, 1) = f2; K2(2,2) =1.0;
/** TODO BERE
* 计算本质矩阵E*/
EssentialMatrix E;
/** Reference
* EssentialMatrix E = K2.transpose() * F * K1;
*/
std::cout<<"EssentialMatrix result is "<<E<<std::endl;
std::cout<<"EssentialMatrix should be: \n"
<<"-0.00490744 -0.0146139 0.34281\n"
<<"0.0212215 -0.000748851 -0.0271105\n"
<<"-0.342111 0.0315182 -0.00552454\n";
/* 本质矩阵求解的是相机之间的相对姿态，第一个相机姿态可以设置为[I|0], 第二个相机的姿态[R|t]
* 可以通过对本质矩阵进行分解来求得, E=U*S*V',其中S是进行尺度归一化之后是diag(1,1,0)
*/
math::Matrix<double, 3, 3> W(0.0);
W(0, 1) = -1.0; W(1, 0) = 1.0; W(2, 2) = 1.0;
math::Matrix<double, 3, 3> Wt(0.0);
Wt(0, 1) = 1.0; Wt(1, 0) = -1.0; Wt(2, 2) = 1.0;
math::Matrix<double, 3, 3> U, S, V;
math::matrix_svd(E, &U, &S, &V);
// 保证旋转矩阵 det(R) = 1 (instead of -1).
if (math::matrix_determinant(U) < 0.0)
for (int i = 0; i < 3; ++i)
U(i,2) = -U(i,2);
if (math::matrix_determinant(V) < 0.0)
for (int i = 0; i < 3; ++i)
V(i,2) = -V(i,2);
/* 相机的姿态一共有4中情况*/
V.transpose();
std::vector<std::pair<math::Matrix3d, math::Vec3d> > poses(4);
poses[0].first = U * W * V;
poses[1].first = U * W * V;
poses[2].first = U * Wt * V;
poses[3].first = U * Wt * V;
poses[0].second = U.col(2);
poses[1].second = -U.col(2);
poses[2].second = U.col(2);
poses[3].second = -U.col(2);
std::cout<<"Result of 4 candidate camera poses shoule be \n"
<<"R0:\n"
<<"-0.985336 0.170469 -0.0072771\n"
<<"0.168777 0.980039 0.105061\n"
<<"0.0250416 0.102292 -0.994439\n"
<<"t0:\n"
<<" 0.0796625 0.99498 0.0605768\n"
<<"R1: \n"
<<"-0.985336 0.170469 -0.0072771\n"
<<"0.168777 0.980039 0.105061\n"
<<"0.0250416 0.102292 -0.994439\n"
<<"t1:\n"
<<"-0.0796625 -0.99498 -0.0605768\n"
<<"R2: \n"
<<"0.999827 -0.0119578 0.0142419\n"
<<"0.0122145 0.999762 -0.0180719\n"
<<"-0.0140224 0.0182427 0.999735\n"
<<"t2:\n"
<<"0.0796625 0.99498 0.0605768\n"
<<"R3: \n"
<<"0.999827 -0.0119578 0.0142419\n"
<<"0.0122145 0.999762 -0.0180719\n"
<<"-0.0140224 0.0182427 0.999735\n"
<<"t3: \n"
<<"-0.0796625 -0.99498 -0.0605768\n";
// 第一个相机的旋转矩阵R1设置为单位矩阵，平移向量t1设置为0
math::Matrix3d R1;
math::matrix_set_identity(&R1);
math::Vec3d t1;
t1.fill(0.0);
// 判断姿态是否合理
bool flags[4];
for(int i=0; i<4; i++){
flags[i] = is_correct_pose(R1, t1, poses[i].first, poses[i].second);
}
//找到正确的姿态
if(flags[0]||flags[1]||flags[2]||flags[3]){
for(int i=0; i<4; i++) {
if(!flags[i])continue;
R = poses[i].first;
t = poses[i].second;
}
return true;
}
return false;
}
int main(int argc, char* argv[]){
FundamentalMatrix F;
F[0] = -0.0051918668202215884;
F[1] = -0.015460923969578466;
F[2] = 0.35260470328319654;
F[3] = 0.022451443619913483;
F[4] = -0.00079225386526248181;
F[5] = -0.027885130552744289;
F[6] = -0.35188558059920161;
F[7] = 0.032418724757766811;
F[8] = -0.005524537443406155;
math::Matrix3d R;
math::Vec3d t;
if(calc_cam_poses(F, f1,f2,R, t)){
std::cout<<"Correct pose found!"<<std::endl;
std::cout<<"R: "<<R<<std::endl;
std::cout<<"t: "<<t<<std::endl;
}
std::cout<<"Result should be: \n";
std::cout<<"R: \n"
<< "0.999827 -0.0119578 0.0142419\n"
<< "0.0122145 0.999762 -0.0180719\n"
<< "-0.0140224 0.0182427 0.999735\n";
std::cout<<"t: \n"
<<"0.0796625 0.99498 0.0605768\n";
return 0;
}
/*
* Copyright (C) 2015, Simon Fuhrmann
* TU Darmstadt - Graphics, Capture and Massively Parallel Computing
* All rights reserved.
*
* This software may be modified and distributed under the terms
* of the BSD 3-Clause license. See the LICENSE.txt file for details.
*/
#include <iostream>
#include <fstream>
#include "util/aligned_memory.h"
#include "util/timer.h"
#include "core/image.h"
#include "core/image_tools.h"
#include "core/image_io.h"
#include "features/surf.h"
#include "features/sift.h"
#include "features/matching.h"
#include "sfm/feature_set.h"
#include "visualizer.h"
core::ByteImage::Ptr
visualize_matching (features::Matching::Result const& matching,
core::ByteImage::Ptr image1, core::ByteImage::Ptr image2,
std::vector<math::Vec2f> const& pos1, std::vector<math::Vec2f> const& pos2)
{
/* Visualize keypoints. */
sfm::Correspondences2D2D vis_matches;
for (std::size_t i = 0; i < matching.matches_1_2.size(); ++i)
{
if (matching.matches_1_2[i] < 0)
continue;
int const j = matching.matches_1_2[i];
sfm::Correspondence2D2D match;
std::copy(pos1[i].begin(), pos1[i].end(), match.p1);
std::copy(pos2[j].begin(), pos2[j].end(), match.p2);
vis_matches.push_back(match);
}
std::cout << "Drawing " << vis_matches.size() << " matches..." << std::endl;
core::ByteImage::Ptr match_image = sfm::Visualizer::draw_matches
(image1, image2, vis_matches);
return match_image;
}
#define DISCRETIZE_DESCRIPTORS 0
template <typename T>
void
convert_sift_descriptors(features::Sift::Descriptors const& sift_descr,
util::AlignedMemory<math::Vector<T, 128> >* aligned_descr)
{
aligned_descr->resize(sift_descr.size());
T* data_ptr = aligned_descr->data()->begin();
for (std::size_t i = 0; i < sift_descr.size(); ++i, data_ptr += 128)
{
sfm::Sift::Descriptor const& d = sift_descr[i];
#if DISCRETIZE_DESCRIPTORS
for (int j = 0; j < 128; ++j)
{
float value = d.data[j];
value = math::clamp(value, 0.0f, 1.0f);
value = math::round(value * 255.0f);
data_ptr[j] = static_cast<unsigned char>(value);
}
#else
std::copy(d.data.begin(), d.data.end(), data_ptr);
#endif
}
}
template <typename T>
void
convert_surf_descriptors(sfm::Surf::Descriptors const& surf_descr,
util::AlignedMemory<math::Vector<T, 64> >* aligned_descr)
{
aligned_descr->resize(surf_descr.size());
T* data_ptr = aligned_descr->data()->begin();
for (std::size_t i = 0; i < surf_descr.size(); ++i, data_ptr += 64)
{
sfm::Surf::Descriptor const& d = surf_descr[i];
#if DISCRETIZE_DESCRIPTORS
for (int j = 0; j < 64; ++j)
{
float value = d.data[j];
value = math::clamp(value, -1.0f, 1.0f);
value = math::round(value * 127.0f);
data_ptr[j] = static_cast<signed char>(value);
}
#else
std::copy(d.data.begin(), d.data.end(), data_ptr);
#endif
}
}
void
feature_set_matching (core::ByteImage::Ptr image1, core::ByteImage::Ptr image2)
{
/*FeatureSet 计算并存储一个视角的特征点，包含SIFT和SURF特征点 */
sfm::FeatureSet::Options feature_set_opts;
//feature_types设置为FEATURE_ALL表示检测SIFT和SURF两种特征点进行匹配
feature_set_opts.feature_types = sfm::FeatureSet::FEATURE_ALL;
feature_set_opts.sift_opts.verbose_output = true;
//feature_set_opts.surf_opts.verbose_output = true;
//feature_set_opts.surf_opts.contrast_threshold = 500.0f;
// 计算第一幅图像的SIT和SURF特征点
sfm::FeatureSet feat1(feature_set_opts);
feat1.compute_features(image1);
// 计算第二幅图像的SIFT和SURF特征点
sfm::FeatureSet feat2(feature_set_opts);
feat2.compute_features(image2);
/* 对sift特征描述子进行匹配 */
// sift 特征描述子匹配参数
sfm::Matching::Options sift_matching_opts;
sift_matching_opts.lowe_ratio_threshold = 0.8f;
sift_matching_opts.descriptor_length = 128;
sift_matching_opts.distance_threshold = std::numeric_limits<float>::max();
#if DISCRETIZE_DESCRIPTORS
util::AlignedMemory<math::Vec128us, 16> sift_descr1, sift_descr2;
#else
util::AlignedMemory<math::Vec128f, 16> sift_descr1, sift_descr2;
#endif
// 将描述子转成特定的内存格式
convert_sift_descriptors(feat1.sift_descriptors, &sift_descr1);
convert_sift_descriptors(feat2.sift_descriptors, &sift_descr2);
// 进行双向匹配
sfm::Matching::Result sift_matching;
sfm::Matching::twoway_match(sift_matching_opts,
sift_descr1.data()->begin(), sift_descr1.size(),
sift_descr2.data()->begin(), sift_descr2.size(),
&sift_matching);
// 去除不一致的匹配对，匹配对feature1和feature2是一致的需要满足，feature1的最近邻居
// 是feature2，feature2的最近邻是feature1
sfm::Matching::remove_inconsistent_matches(&sift_matching);
std::cout << "Consistent Sift Matches: "
<< sfm::Matching::count_consistent_matches(sift_matching)
<< std::endl;
/* 对surf特征描述子进行匹配 */
// surf特征匹配参数
sfm::Matching::Options surf_matching_opts;
// 最近邻和次近邻的比
surf_matching_opts.lowe_ratio_threshold = 0.7f;
// 特征描述子的维度
surf_matching_opts.descriptor_length = 64;
surf_matching_opts.distance_threshold = std::numeric_limits<float>::max();
#if DISCRETIZE_DESCRIPTORS
util::AlignedMemory<math::Vec64s, 16> surf_descr1, surf_descr2;
#else
util::AlignedMemory<math::Vec64f, 16> surf_descr1, surf_descr2;
#endif
// 将描述子转化成特殊的格式
convert_surf_descriptors(feat1.surf_descriptors, &surf_descr1);
convert_surf_descriptors(feat2.surf_descriptors, &surf_descr2);
// 进行surf描述子的双向匹配
sfm::Matching::Result surf_matching;
sfm::Matching::twoway_match(surf_matching_opts,
surf_descr1.data()->begin(), surf_descr1.size(),
surf_descr2.data()->begin(), surf_descr2.size(),
&surf_matching);
// 去除不一致的匹配对，匹配对feature 1 和 feature 2 互为最近邻
sfm::Matching::remove_inconsistent_matches(&surf_matching);
std::cout << "Consistent Surf Matches: "
<< sfm::Matching::count_consistent_matches(surf_matching)
<< std::endl;
// 对sift匹配的结果和surf匹配的结果进行融合
sfm::Matching::Result matching;
sfm::Matching::combine_results(sift_matching, surf_matching, &matching);
std::cout << "Consistent Matches: "
<< sfm::Matching::count_consistent_matches(matching)
<< std::endl;
/* 特征匹配可视化 */
/* Draw features. */
std::vector<sfm::Visualizer::Keypoint> features1;
for (std::size_t i = 0; i < feat1.sift_descriptors.size(); ++i)
{
if (matching.matches_1_2[i] == -1)
continue;
sfm::Sift::Descriptor const& descr = feat1.sift_descriptors[i];
sfm::Visualizer::Keypoint kp;
kp.orientation = descr.orientation;
kp.radius = descr.scale * 3.0f;
kp.x = descr.x;
kp.y = descr.y;
features1.push_back(kp);
}
std::vector<sfm::Visualizer::Keypoint> features2;
for (std::size_t i = 0; i < feat2.sift_descriptors.size(); ++i)
{
if (matching.matches_2_1[i] == -1)
continue;
sfm::Sift::Descriptor const& descr = feat2.sift_descriptors[i];
sfm::Visualizer::Keypoint kp;
kp.orientation = descr.orientation;
kp.radius = descr.scale * 3.0f;
kp.x = descr.x;
kp.y = descr.y;
features2.push_back(kp);
}
image1 = sfm::Visualizer::draw_keypoints(image1,
features1, sfm::Visualizer::RADIUS_BOX_ORIENTATION);
image2 = sfm::Visualizer::draw_keypoints(image2,
features2, sfm::Visualizer::RADIUS_BOX_ORIENTATION);
core::ByteImage::Ptr match_image = visualize_matching(
matching, image1, image2, feat1.positions, feat2.positions);
std::string output_filename = "./tmp/matching_featureset.png";
std::cout << "Saving visualization to " << output_filename << std::endl;
core::image::save_file(match_image, output_filename);
}
int
main (int argc, char** argv)
{
if (argc < 3)
{
std::cerr << "Syntax: " << argv[0] << " image1 image2" << std::endl;
return 1;
}
// 用于加速
#ifdef __SSE2__
std::cout << "SSE2 is enabled!" << std::endl;
#endif
#ifdef __SSE3__
std::cout << "SSE3 is enabled!" << std::endl;
#endif
/* Regular two-view matching. */
core::ByteImage::Ptr image1, image2;
try
{
std::cout << "Loading " << argv[1] << "..." << std::endl;
image1 = core::image::load_file(std::string(argv[1]));
// 图像尺寸减半
image1 = core::image::rescale_half_size<uint8_t>(image1);
//image1 = core::image::rescale_half_size<uint8_t>(image1);
//image1 = core::image::rotate<uint8_t>(image1, core::image::ROTATE_CCW);
std::cout << "Loading " << argv[2] << "..." << std::endl;
image2 = core::image::load_file(argv[2]);
// 图像尺寸减半
image2 = core::image::rescale_half_size<uint8_t>(image2);
//image2 = core::image::rescale_half_size<uint8_t>(image2);
//image2 = core::image::rotate<uint8_t>(image2, core::image::ROTATE_CCW);
}
catch (std::exception& e)
{
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
// 进行特征提取和特征匹配
feature_set_matching(image1, image2);
return 0;
}
/*
* Copyright (C) 2015, Simon Fuhrmann
* TU Darmstadt - Graphics, Capture and Massively Parallel Computing
* All rights reserved.
*
* This software may be modified and distributed under the terms
* of the BSD 3-Clause license. See the LICENSE.txt file for details.
*/
#include <iostream>
#include "util/file_system.h"
#include "util/timer.h"
#include "core/image.h"
#include "core/image_tools.h"
#include "core/image_io.h"
#include "features/surf.h"
#include "features/sift.h"
#include "visualizer.h"
bool
sift_compare (features::Sift::Descriptor const& d1, features::Sift::Descriptor const& d2)
{
return d1.scale > d2.scale;
}
int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "Syntax: " << argv[0] << " <image>" << std::endl;
return 1;
}
/* 加载图像*/
core::ByteImage::Ptr image;
std::string image_filename = argv[1];
try
{
std::cout << "Loading " << image_filename << "..." << std::endl;
image = core::image::load_file(image_filename);
//image = core::image::rescale_half_size<uint8_t>(image);
//image = core::image::rescale_half_size<uint8_t>(image);
}
catch (std::exception& e)
{
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
/* SIFT 特征检测. */
features::Sift::Descriptors sift_descr;
features::Sift::Keypoints sift_keypoints;
{
features::Sift::Options sift_options;
sift_options.verbose_output = true;
sift_options.debug_output = true;
features::Sift sift(sift_options);
sift.set_image(image);
util::WallTimer timer;
sift.process(); // 主程序
std::cout << "Computed SIFT features in "
<< timer.get_elapsed() << "ms." << std::endl;
sift_descr = sift.get_descriptors();
sift_keypoints = sift.get_keypoints();
}
// 对特征点按照尺度进行排序
std::sort(sift_descr.begin(), sift_descr.end(), sift_compare);
std::vector<features::Visualizer::Keypoint> sift_drawing;
for (std::size_t i = 0; i < sift_descr.size(); ++i)
{
features::Visualizer::Keypoint kp;
kp.orientation = sift_descr[i].orientation;
kp.radius = sift_descr[i].scale;
kp.x = sift_descr[i].x;
kp.y = sift_descr[i].y;
sift_drawing.push_back(kp);
}
core::ByteImage::Ptr sift_image = features::Visualizer::draw_keypoints(image,
sift_drawing, features::Visualizer::RADIUS_BOX_ORIENTATION);
/* 保存图像文件名 */
std::string sift_out_fname = "./tmp/" + util::fs::replace_extension
(util::fs::basename(image_filename), "sift.png");
std::cout << "保存图像: " << sift_out_fname << std::endl;
core::image::save_file(sift_image, sift_out_fname);
return 0;
}
扫一扫
1282
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
