/// Triangulate a set of points between two view
void triangulate2View_Vector(const Mat34 & P1,
const Mat34 & P2,
const std::vector<SIOPointFeature> & vec_feat1,
const std::vector<SIOPointFeature> & vec_feat2,
const std::vector<IndMatch> & vec_index,
std::vector<Vec3> * pvec_3dPoint,
std::vector<double> * pvec_residual)
{
assert(pvec_3dPoint);
assert(pvec_residual);
pvec_3dPoint->reserve(vec_index.size());
pvec_residual->reserve(vec_index.size());
for (size_t i=0; i < vec_index.size(); ++i)
{
//Get corresponding point and triangulate it
const SIOPointFeature & imaA = vec_feat1[vec_index[i]._i];
const SIOPointFeature & imaB = vec_feat2[vec_index[i]._j];
const Vec2 x1 = imaA.coords().cast<double>(),
x2 = imaB.coords().cast<double>();
Vec3 X_euclidean = Vec3::Zero();
TriangulateDLT(P1, x1, P2, x2, &X_euclidean);
double dResidual2D =
(PinholeCamera::Residual(P1, X_euclidean, x1) +
PinholeCamera::Residual(P2, X_euclidean, x2)) /2.0;
// store 3DPoint and associated residual
pvec_3dPoint->push_back(X_euclidean);
pvec_residual->push_back(dResidual2D);
}
if (!vec_index.empty())
{
double dMin = *min_element(pvec_residual->begin(), pvec_residual->end()),
dMax = *max_element(pvec_residual->begin(), pvec_residual->end());
std::cout << std::endl
<< "SfMRobust::triangulate2View_Vector" << std::endl
<< "\t-- Residual min max -- " << dMin <<"\t" << dMax << std::endl;
}
}
int TriangulatePoints(const std::vector<cv::Vec2d> &kp0,
const std::vector<cv::Vec2d> &kp1, const cv::Mat &K,
const cv::Mat &R0, const cv::Mat &t0, const cv::Mat &R1,
const cv::Mat &t1, std::vector<cv::Point3f> &points3d,
std::vector<bool> &points3d_mask) {
bool generate_test = false;
if (generate_test) {
cv::FileStorage file("triangulation_test_data.txt", cv::FileStorage::WRITE);
file << "NumPoints" << (int)kp0.size();
write(file, "kp0", kp0);
write(file, "kp1", kp1);
file << "K" << K << "R0" << R0 << "t0" << t0 << "R1" << R1 << "t1" << t1;
/*
cv::FileStorage file_read;
file_read.open("triangulation_test_data.txt", cv::FileStorage::READ);
int kp_num = (int)file_read["NumPoints"];
FileNode kp0node = file_read["kp0"];
std::vector<cv::Vec2d> kp0_new;
std::vector<cv::Vec2d> kp1_new;
cv::Mat K_new, R0_new, t0_new, R1_new, t1_new;
read(file_read["kp0"], kp0_new);
read(file_read["kp1"], kp1_new);
file_read["K"] >> K_new;
file_read["R0"] >> R0_new;
file_read["t0"] >> t0_new;
file_read["R1"] >> R1_new;
file_read["t1"] >> t1_new;
*/
}
double reprojection_error_thres = 5.0;
cv::Mat P0, P1;
RtToP(R0, t0, P0);
RtToP(R1, t1, P1);
P0 = K * P0;
P1 = K * P1;
// TODO: Why
cv::Mat center0 = -R0.t() * t0;
cv::Mat center1 = -R1.t() * t1;
int nParallal = 0;
int nInfinite = 0;
int nLargeError = 0;
int nNegativeDepth = 0;
int nGood = 0;
points3d.resize(kp0.size());
points3d_mask.resize(kp0.size(), true);
for (int i = 0; i < kp0.size(); ++i) {
TriangulateDLT(kp0[i], kp1[i], P0, P1, points3d[i]);
cv::Mat p_global(3, 1, CV_64F);
p_global.at<double>(0) = points3d[i].x;
p_global.at<double>(1) = points3d[i].y;
p_global.at<double>(2) = points3d[i].z;
cv::Mat p3dC0 = R0 * p_global + t0;
cv::Mat p3dC1 = R1 * p_global + t1;
double depth0 = p3dC0.at<double>(2);
double depth1 = p3dC1.at<double>(2);
if (depth0 <= 0 || depth1 <= 0) {
points3d_mask[i] = false;
nNegativeDepth++;
continue;
}
// TODO: Make sure isfinite is in std
if (!std::isfinite(p3dC0.at<double>(0)) ||
!std::isfinite(p3dC0.at<double>(1)) ||
!std::isfinite(p3dC0.at<double>(2))) {
nInfinite++;
points3d_mask[i] = false;
continue;
}
// TODO: Make sure isfinite is in std
if (!std::isfinite(p3dC1.at<double>(0)) ||
!std::isfinite(p3dC1.at<double>(1)) ||
!std::isfinite(p3dC1.at<double>(2))) {
nInfinite++;
points3d_mask[i] = false;
continue;
}
// Check parallax
cv::Mat p0_vec = p3dC0 - center0;
cv::Mat p1_vec = p3dC1 - center1;
double dist0 = cv::norm(p0_vec);
double dist1 = cv::norm(p1_vec);
double cosParallax = p0_vec.dot(p1_vec) / (dist0 * dist1);
if (cosParallax > 0.998) {
nParallal++;
points3d_mask[i] = false;
continue;
}
double error0 = ComputeReprojectionError(points3d[i], kp0[i], P0);
double error1 = ComputeReprojectionError(points3d[i], kp1[i], P1);
if (error0 > reprojection_error_thres ||
error1 > reprojection_error_thres) {
nLargeError++;
points3d_mask[i] = false;
continue;
}
nGood++;
}
std::cout << "Found " << nGood << " / " << kp0.size()
<< " good triangulated points.\n";
std::cout << "Found " << nInfinite << " / " << kp0.size()
<< " infinite points during triangulation.\n";
std::cout << "Found " << nParallal << " / " << kp0.size()
<< " parallal points during triangulation.\n";
std::cout << "Found " << nLargeError << " / " << kp0.size()
<< " large error points during triangulation.\n";
std::cout << "Found " << nNegativeDepth << " / " << kp0.size()
<< " negative depth points during triangulation.\n";
return nGood;
}
/// From the essential matrix test the 4 possible solutions
/// and return the best one. (point in front of the selected solution)
bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2,
const Mat & x1, const Mat & x2,
const Mat3 & E, const std::vector<size_t> & vec_inliers,
Mat3 * R, Vec3 * t)
{
bool bOk = false;
// Accumulator to find the best solution
std::vector<size_t> f(4, 0);
std::vector<Mat3> Es; // Essential,
std::vector<Mat3> Rs; // Rotation matrix.
std::vector<Vec3> ts; // Translation matrix.
Es.push_back(E);
// Recover best rotation and translation from E.
MotionFromEssential(E, &Rs, &ts);
//-> Test the 4 solutions will all the point
assert(Rs.size() == 4);
assert(ts.size() == 4);
Mat34 P1, P2;
Mat3 R1 = Mat3::Identity();
Vec3 t1 = Vec3::Zero();
P_From_KRt(K1, R1, t1, &P1);
for (int i = 0; i < 4; ++i) {
const Mat3 &R2 = Rs[i];
const Vec3 &t2 = ts[i];
P_From_KRt(K2, R2, t2, &P2);
Vec3 X;
for (size_t k = 0; k < vec_inliers.size(); ++k)
{
const Vec2 & x1_ = x1.col(vec_inliers[k]);
const Vec2 & x2_ = x2.col(vec_inliers[k]);
TriangulateDLT(P1, x1_, P2, x2_, &X);
// Test if point is front to the two cameras.
if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) {
++f[i];
}
}
}
// Check the solution :
std::cout << "\t Number of points in front of both cameras:"
<< f[0] << " " << f[1] << " " << f[2] << " " << f[3] << std::endl;
std::vector<size_t>::iterator iter = max_element(f.begin(), f.end());
if(*iter != 0)
{
size_t index = std::distance(f.begin(), iter);
(*R) = Rs[index];
(*t) = ts[index];
bOk = true;
}
else {
std::cerr << std::endl << "/!\\There is no right solution,"
<<" probably intermediate results are not correct or no points"
<<" in front of both cameras" << std::endl;
bOk = false;
}
return bOk;
}
