TEST(TinyMatrix, LookAt) {
// Simple orthogonality check.
Vec3 e; e[0]= 1; e[1] = 2; e[2] = 3;
Mat3 R = LookAt(e);
Mat3 I = Mat3::Identity();
Mat3 RRT = R*R.transpose();
Mat3 RTR = R.transpose()*R;
EXPECT_MATRIX_NEAR(I, RRT, 1e-15);
EXPECT_MATRIX_NEAR(I, RTR, 1e-15);
}
TEST(TinyMatrix, LookAt) {
// 简单的正交验证
Vec3 e; e[0]= 1; e[1] = 2; e[2] = 3;
Mat3 R = LookAt(e);//这个R是旋转矩阵，则R为正交矩阵，R与R的转置相乘为单位阵
Mat3 I = Mat3::Identity();
Mat3 RRT = R*R.transpose();
Mat3 RTR = R.transpose()*R;
EXPECT_MATRIX_NEAR(I, RRT, 1e-15);
EXPECT_MATRIX_NEAR(I, RTR, 1e-15);
}
SimpleCamera(
const Mat3 & K = Mat3::Identity(),
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero())
: _K(K), _R(R), _t(t)
{
_C = -R.transpose() * t;
P_From_KRt(_K, _R, _t, &_P);
}
BrownPinholeCamera(
double focal,
double ppx,
double ppy,
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero(),
double k1 = 0.0,
double k2 = 0.0,
double k3 = 0.0)
: _R(R), _t(t), _f(focal), _ppx(ppx), _ppy(ppy),
_k1(k1), _k2(k2), _k3(k3)
{
_C = -R.transpose() * t;
_K << _f, 0, _ppx,
0, _f, _ppy,
0, 0, 1;
P_From_KRt(_K, _R, _t, &_P);
}
bool robustRelativePose(
const Mat3 & K1, const Mat3 & K2,
const Mat & x1, const Mat & x2,
RelativePose_Info & relativePose_info,
const std::pair<size_t, size_t> & size_ima1,
const std::pair<size_t, size_t> & size_ima2,
const size_t max_iteration_count)
{
// Use the 5 point solver to estimate E
typedef openMVG::essential::kernel::FivePointKernel SolverType;
// Define the AContrario adaptor
typedef ACKernelAdaptorEssential<
SolverType,
openMVG::fundamental::kernel::EpipolarDistanceError,
UnnormalizerT,
Mat3>
KernelType;
KernelType kernel(x1, size_ima1.first, size_ima1.second,
x2, size_ima2.first, size_ima2.second, K1, K2);
// Robustly estimation of the Essential matrix and it's precision
std::pair<double,double> acRansacOut = ACRANSAC(kernel, relativePose_info.vec_inliers,
max_iteration_count, &relativePose_info.essential_matrix, relativePose_info.initial_residual_tolerance, false);
relativePose_info.found_residual_precision = acRansacOut.first;
if (relativePose_info.vec_inliers.size() < 2.5 * SolverType::MINIMUM_SAMPLES )
return false; // no sufficient coverage (the model does not support enough samples)
// estimation of the relative poses
Mat3 R;
Vec3 t;
if (!estimate_Rt_fromE(
K1, K2, x1, x2,
relativePose_info.essential_matrix, relativePose_info.vec_inliers, &R, &t))
return false; // cannot find a valid [R|t] couple that makes the inliers in front of the camera.
// Store [R|C] for the second camera, since the first camera is [Id|0]
relativePose_info.relativePose = geometry::Pose3(R, -R.transpose() * t);
return true;
}
// Generates all necessary inputs and expected outputs for EuclideanResection.
void CreateCameraSystem(const Mat3& KK,
const Mat3X& x_image,
const Vec& X_distances,
const Mat3& R_input,
const Vec3& T_input,
Mat2X *x_camera,
Mat3X *X_world,
Mat3 *R_expected,
Vec3 *T_expected) {
int num_points = x_image.cols();
Mat3X x_unit_cam(3, num_points);
x_unit_cam = KK.inverse() * x_image;
// Create normalized camera coordinates to be used as an input to the PnP
// function, instead of using NormalizeColumnVectors(&x_unit_cam).
*x_camera = x_unit_cam.block(0, 0, 2, num_points);
for (int i = 0; i < num_points; ++i){
x_unit_cam.col(i).normalize();
}
// Create the 3D points in the camera system.
Mat X_camera(3, num_points);
for (int i = 0; i < num_points; ++i) {
X_camera.col(i) = X_distances(i) * x_unit_cam.col(i);
}
// Apply the transformation to the camera 3D points
Mat translation_matrix(3, num_points);
translation_matrix.row(0).setConstant(T_input(0));
translation_matrix.row(1).setConstant(T_input(1));
translation_matrix.row(2).setConstant(T_input(2));
*X_world = R_input * X_camera + translation_matrix;
// Create the expected result for comparison.
*R_expected = R_input.transpose();
*T_expected = *R_expected * ( - T_input);
};
void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) {
// Decompose using the RQ decomposition HZ A4.1.1 pag.579.
Mat3 K = P.block(0, 0, 3, 3);
Mat3 Q;
Q.setIdentity();
// Set K(2,1) to zero.
if (K(2,1) != 0) {
double c = -K(2,2);
double s = K(2,1);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qx;
Qx << 1, 0, 0,
0, c, -s,
0, s, c;
K = K * Qx;
Q = Qx.transpose() * Q;
}
// Set K(2,0) to zero.
if (K(2,0) != 0) {
double c = K(2,2);
double s = K(2,0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qy;
Qy << c, 0, s,
0, 1, 0,
-s, 0, c;
K = K * Qy;
Q = Qy.transpose() * Q;
}
// Set K(1,0) to zero.
if (K(1,0) != 0) {
double c = -K(1,1);
double s = K(1,0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qz;
Qz << c,-s, 0,
s, c, 0,
0, 0, 1;
K = K * Qz;
Q = Qz.transpose() * Q;
}
Mat3 R = Q;
//Mat3 H = P.block(0, 0, 3, 3);
// RQ decomposition
//Eigen::HouseholderQR<Mat3> qr(H);
//Mat3 K = qr.matrixQR().triangularView<Eigen::Upper>();
//Mat3 R = qr.householderQ();
// Ensure that the diagonal is positive and R determinant == 1.
if (K(2,2) < 0) {
K = -K;
R = -R;
}
if (K(1,1) < 0) {
Mat3 S;
S << 1, 0, 0,
0,-1, 0,
0, 0, 1;
K = K * S;
R = S * R;
}
if (K(0,0) < 0) {
Mat3 S;
S << -1, 0, 0,
0, 1, 0,
0, 0, 1;
K = K * S;
R = S * R;
}
// Compute translation.
Eigen::PartialPivLU<Mat3> lu(K);
Vec3 t = lu.solve(P.col(3));
if(R.determinant()<0) {
R = -R;
t = -t;
}
// scale K so that K(2,2) = 1
K = K / K(2,2);
*Kp = K;
*Rp = R;
*tp = t;
}
void MainWindow::registerProject()
{
if (m_doc._sfm_data.control_points.size() < 3)
{
QMessageBox msgBox;
msgBox.setText("At least 3 control points are required.");
msgBox.exec();
return;
}
// Assert that control points can be triangulated
for (Landmarks::const_iterator iterL = m_doc._sfm_data.control_points.begin();
iterL != m_doc._sfm_data.control_points.end(); ++iterL)
{
if (iterL->second.obs.size() < 2)
{
QMessageBox msgBox;
msgBox.setText("Each control point must be defined in at least 2 pictures.");
msgBox.exec();
return;
}
}
//---
// registration (coarse):
// - compute the 3D points corresponding to the control point observation for the SfM scene
// - compute a coarse registration between the controls points & the triangulated point
// - transform the scene according the found transformation
//---
std::map<IndexT, Vec3> vec_control_points, vec_triangulated;
std::map<IndexT, double> vec_triangulation_errors;
for (Landmarks::iterator iterCP = m_doc._sfm_data.control_points.begin();
iterCP != m_doc._sfm_data.control_points.end(); ++iterCP)
{
Landmark & landmark = iterCP->second;
//Triangulate the point:
Triangulation trianObj;
const Observations & obs = landmark.obs;
for(Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
const View * view = m_doc._sfm_data.views.at(itObs->first).get();
if (!m_doc._sfm_data.IsPoseAndIntrinsicDefined(view))
continue;
const openMVG::cameras::IntrinsicBase * cam = m_doc._sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
const openMVG::geometry::Pose3 pose = m_doc._sfm_data.GetPoseOrDie(view);
trianObj.add(
cam->get_projective_equivalent(pose),
cam->get_ud_pixel(itObs->second.x));
}
// Compute the 3D point
const Vec3 X = trianObj.compute();
if (trianObj.minDepth() > 0) // Keep the point only if it have a positive depth
{
vec_triangulated[iterCP->first] = X;
vec_control_points[iterCP->first] = landmark.X;
vec_triangulation_errors[iterCP->first] = trianObj.error()/(double)trianObj.size();
}
else
{
std::cout << "Invalid triangulation" << std::endl;
return;
}
}
if (vec_control_points.size() < 3)
{
QMessageBox msgBox;
msgBox.setText("Insufficient number of triangulated control points.");
msgBox.exec();
return;
}
// compute the similarity
{
// data conversion to appropriate container
Mat x1(3, vec_control_points.size()),
x2(3, vec_control_points.size());
for (int i=0; i < vec_control_points.size(); ++i)
{
x1.col(i) = vec_triangulated[i];
x2.col(i) = vec_control_points[i];
}
std::cout
<< "Control points observation triangulations:\n"
<< x1 << std::endl << std::endl
<< "Control points coords:\n"
<< x2 << std::endl << std::endl;
Vec3 t;
Mat3 R;
double S;
if (openMVG::geometry::FindRTS(x1, x2, &S, &t, &R))
{
openMVG::geometry::Refine_RTS(x1,x2,&S,&t,&R);
std::cout << "Found transform:\n"
<< " scale: " << S << "\n"
<< " rotation:\n" << R << "\n"
<< " translation: "<< t.transpose() << std::endl;
// Encode the transformation as a 3D Similarity transformation matrix // S * R * X + t
const openMVG::geometry::Similarity3 sim(geometry::Pose3(R, -R.transpose() * t/S), S);
//--
// Apply the found transformation
//--
// Transform the landmark positions
for (Landmarks::iterator iterL = m_doc._sfm_data.structure.begin();
iterL != m_doc._sfm_data.structure.end(); ++iterL)
{
iterL->second.X = sim(iterL->second.X);
}
// Transform the camera positions
for (Poses::iterator iterP = m_doc._sfm_data.poses.begin();
iterP != m_doc._sfm_data.poses.end(); ++iterP)
{
geometry::Pose3 & pose = iterP->second;
pose = sim(pose);
}
// Display some statistics:
std::stringstream os;
for (Landmarks::const_iterator iterL = m_doc._sfm_data.control_points.begin();
iterL != m_doc._sfm_data.control_points.end(); ++iterL)
{
const IndexT CPIndex = iterL->first;
// If the control point has not been used, continue...
if (vec_triangulation_errors.find(CPIndex) == vec_triangulation_errors.end())
continue;
os
<< "CP index: " << CPIndex << "\n"
<< "CP triangulation error: " << vec_triangulation_errors[CPIndex] << " pixel(s)\n"
<< "CP registration error: "
<< (sim(vec_triangulated[CPIndex]) - vec_control_points[CPIndex]).norm() << " user unit(s)"<< "\n\n";
}
std::cout << os.str();
QMessageBox msgBox;
msgBox.setText(QString::fromStdString(string_pattern_replace(os.str(), "\n", "<br>")));
msgBox.exec();
}
else
{
QMessageBox msgBox;
msgBox.setText("Registration failed. Please check your Control Points coordinates.");
msgBox.exec();
}
}
}
// Inverse
Pose3 inverse() const
{
return Pose3(_rotation.transpose(), -(_rotation * _center));
}
int main(int argc, char **argv)
{
enum
{
ROBUST_RIGID_REGISTRATION = 0,
RIGID_REGISTRATION_ALL_POINTS = 1
};
std::string
sSfM_Data_Filename_In,
sSfM_Data_Filename_Out;
unsigned int rigid_registration_method = RIGID_REGISTRATION_ALL_POINTS;
CmdLine cmd;
cmd.add(make_option('i', sSfM_Data_Filename_In, "input_file"));
cmd.add(make_option('o', sSfM_Data_Filename_Out, "output_file"));
cmd.add(make_option('m', rigid_registration_method, "method"));
try
{
if (argc == 1) throw std::string("Invalid command line parameter.");
cmd.process(argc, argv);
}
catch(const std::string& s)
{
std::cerr
<< "Usage: " << argv[0] << '\n'
<< " GPS registration of a SfM Data scene,\n"
<< "[-i|--input_file] path to the input SfM_Data scene\n"
<< "[-o|--output_file] path to the output SfM_Data scene\n"
<< "[-m|--method] method to use for the rigid registration\n"
<< "\t0 => registration is done using a robust estimation,\n"
<< "\t1 (default)=> registration is done using all points.\n"
<< std::endl;
std::cerr << s << std::endl;
return EXIT_FAILURE;
}
if (sSfM_Data_Filename_In.empty() || sSfM_Data_Filename_Out.empty())
{
std::cerr << "Invalid input or output filename." << std::endl;
return EXIT_FAILURE;
}
//
// Load a SfM scene
// For each valid view (pose & intrinsic defined)
// - iff a GPS position can be parsed
// - store corresponding camera pose & GPS position
// - Compute the registration between the selected camera poses & GPS positions
// Load input SfM_Data scene
SfM_Data sfm_data;
if (!Load(sfm_data, sSfM_Data_Filename_In, ESfM_Data(ALL)))
{
std::cerr
<< "\nThe input SfM_Data file \"" << sSfM_Data_Filename_In
<< "\" cannot be read." << std::endl;
return EXIT_FAILURE;
}
// Init the EXIF reader (will be used for GPS data reading)
std::unique_ptr<Exif_IO> exifReader(new Exif_IO_EasyExif);
if (!exifReader)
{
std::cerr << "Cannot instantiate the EXIF metadata reader." << std::endl;
return EXIT_FAILURE;
}
// List corresponding poses (SfM - GPS)
std::vector<Vec3> vec_sfm_center, vec_gps_center;
for (const auto & view_it : sfm_data.GetViews() )
{
if (!sfm_data.IsPoseAndIntrinsicDefined(view_it.second.get()))
continue;
const std::string view_filename =
stlplus::create_filespec(sfm_data.s_root_path, view_it.second->s_Img_path);
// Try to parse EXIF metada & check existence of EXIF data
if (! (exifReader->open( view_filename ) &&
exifReader->doesHaveExifInfo()) )
continue;
// Check existence of GPS coordinates
double latitude, longitude, altitude;
if ( exifReader->GPSLatitude( &latitude ) &&
exifReader->GPSLongitude( &longitude ) &&
exifReader->GPSAltitude( &altitude ) )
{
// Add ECEF XYZ position to the GPS position array
vec_gps_center.push_back( lla_to_ecef( latitude, longitude, altitude ) );
const openMVG::geometry::Pose3 pose(sfm_data.GetPoseOrDie(view_it.second.get()));
vec_sfm_center.push_back( pose.center() );
}
}
if ( vec_sfm_center.empty() )
{
std::cerr << "No valid corresponding GPS data found for the used views." << std::endl;
return EXIT_FAILURE;
}
std::cout << std::endl
<< "Registration report:\n"
<< " #corresponding SFM - GPS data: " << vec_sfm_center.size() << "\n"
<< std::endl;
// Export the corresponding poses (for debugging & see the transformation)
plyHelper::exportToPly( vec_gps_center,
stlplus::create_filespec(stlplus::folder_part(sSfM_Data_Filename_Out), "GPS_position", "ply"));
plyHelper::exportToPly( vec_sfm_center,
stlplus::create_filespec(stlplus::folder_part(sSfM_Data_Filename_Out), "SFM_position", "ply"));
{
// Convert positions to the appropriate data container
const Mat X_SfM = Eigen::Map<Mat>(vec_sfm_center[0].data(), 3, vec_sfm_center.size());
const Mat X_GPS = Eigen::Map<Mat>(vec_gps_center[0].data(), 3, vec_gps_center.size());
openMVG::geometry::Similarity3 sim;
// Compute the registration:
// - using a rigid scheme (using all points)
// - using a robust scheme (using partial points - robust estimation)
switch (rigid_registration_method)
{
case ROBUST_RIGID_REGISTRATION:
{
using namespace openMVG::robust;
using namespace openMVG::geometry;
geometry::kernel::Similarity3_Kernel kernel(X_SfM, X_GPS);
const double lmeds_median = LeastMedianOfSquares
(
kernel,
&sim
);
std::cout << "LMeds found a model with an upper bound of: " << sqrt(lmeds_median) << " user units."<< std::endl;
// Compute & display fitting errors
{
const Vec vec_fitting_errors_eigen(
geometry::kernel::Similarity3ErrorSquaredMetric::ErrorVec(sim, X_SfM, X_GPS).array().sqrt());
std::cout << "\n3D Similarity fitting error using all points (in target coordinate system units):";
minMaxMeanMedian<float>(
vec_fitting_errors_eigen.data(),
vec_fitting_errors_eigen.data() + vec_fitting_errors_eigen.rows() );
}
// INLIERS only
{
std::vector<float> vec_fitting_errors;
for (Mat::Index i = 0; i < X_SfM.cols(); ++i)
{
if (geometry::kernel::Similarity3ErrorSquaredMetric::Error(sim, X_SfM.col(i), X_GPS.col(i)) < lmeds_median)
vec_fitting_errors.push_back((X_GPS.col(i) - sim(X_SfM.col(i))).norm());
}
std::cout << "\nFound: " << vec_fitting_errors.size() << " inliers"
<< " from " << X_SfM.cols() << " points." << std::endl;
std::cout << "\n3D Similarity fitting error using only the fitted inliers (in target coordinate system units):";
minMaxMeanMedian<float>( vec_fitting_errors.begin(), vec_fitting_errors.end() );
}
}
break;
case RIGID_REGISTRATION_ALL_POINTS:
{
Vec3 t;
Mat3 R;
double S;
if (!openMVG::geometry::FindRTS(X_SfM, X_GPS, &S, &t, &R))
{
std::cerr << "Failed to comute the registration" << std::endl;
return EXIT_FAILURE;
}
std::cout
<< "Found transform:\n"
<< " scale: " << S << "\n"
<< " rotation:\n" << R << "\n"
<< " translation: " << std::fixed << std::setprecision(9)
<< t.transpose() << std::endl;
// Encode the transformation as a 3D Similarity transformation matrix // S * R * X + t
sim = openMVG::geometry::Similarity3(geometry::Pose3(R, -R.transpose()* t/S), S);
// Compute & display fitting errors
{
const Vec vec_fitting_errors_eigen(
geometry::kernel::Similarity3ErrorSquaredMetric::ErrorVec(sim, X_SfM, X_GPS).array().sqrt());
std::cout << "\n3D Similarity fitting error (in target coordinate system units):";
minMaxMeanMedian<float>(
vec_fitting_errors_eigen.data(),
vec_fitting_errors_eigen.data() + vec_fitting_errors_eigen.rows() );
}
}
break;
default:
std::cerr << "Unknow rigid registration method" << std::endl;
return EXIT_FAILURE;
}
//--
// Apply the found transformation to the SfM Data Scene
//--
openMVG::sfm::ApplySimilarity(sim, sfm_data);
}
// Export the SfM_Data scene in the expected format
if (Save(
sfm_data,
sSfM_Data_Filename_Out.c_str(),
ESfM_Data(ALL)))
{
return EXIT_SUCCESS;
}
else
{
std::cerr
<< std::endl
<< "An error occured while trying to save \""
<< sSfM_Data_Filename_Out << "\"." << std::endl;
return EXIT_FAILURE;
}
return EXIT_FAILURE;
}
void DecomposeProjectionMatrix(const Mat& P, Mat3* K, Mat3* R, Vec3* t)
{
// This is a modified version of the function "KRt_From_P", found in libmv and openMVG.
// It is subject to the terms of the Mozilla Public License, v. 2.0, a copy of the MPL
// can be found under "license.openMVG"
// Decompose using the RQ decomposition HZ A4.1.1 pag.579.
Mat3 Kp = P.block(0, 0, 3, 3);
Mat3 Q; Q.setIdentity();
// Set K(2, 1) to zero.
if (Kp(2, 1) != 0)
{
double c = -Kp(2, 2);
double s = Kp(2, 1);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qx;
Qx << 1, 0, 0,
0, c, -s,
0, s, c;
Kp = Kp * Qx;
Q = Qx.transpose() * Q;
}
// Set K(2, 0) to zero.
if (Kp(2, 0) != 0)
{
double c = Kp(2, 2);
double s = Kp(2, 0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qy;
Qy << c, 0, s,
0, 1, 0,
-s, 0, c;
Kp = Kp * Qy;
Q = Qy.transpose() * Q;
}
// Set K(1, 0) to zero.
if (Kp(1, 0) != 0)
{
double c = -Kp(1, 1);
double s = Kp(1, 0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qz;
Qz << c, -s, 0,
s, c, 0,
0, 0, 1;
Kp = Kp * Qz;
Q = Qz.transpose() * Q;
}
Mat3 Rp = Q;
// Ensure that the diagonal is positive and R determinant == 1
if (Kp(2, 2) < 0)
{
Kp = -Kp;
Rp = -Rp;
}
if (Kp(1, 1) < 0)
{
Mat3 S;
S << 1, 0, 0,
0, -1, 0,
0, 0, 1;
Kp = Kp * S;
Rp = S * Rp;
}
if (Kp(0, 0) < 0)
{
Mat3 S;
S << -1, 0, 0,
0, 1, 0,
0, 0, 1;
Kp = Kp * S;
Rp = S * Rp;
}
// Compute translation.
Vec3 tp = Kp.colPivHouseholderQr().solve(P.col(3));
if(Rp.determinant() < 0)
{
Rp = -Rp;
tp = -tp;
}
// Scale K so that K(2, 2) = 1
Kp = Kp / Kp(2, 2);
*K = Kp;
*R = Rp;
*t = tp;
}
bool FindExtrinsics(const Mat3& E, const Point2Vec& pts1, const Point2Vec& pts2, Mat3* R, Vec3* t)
{
const auto num_correspondences = pts1.size();
// Put first camera at origin
Mat3 R0; R0.setIdentity();
Vec3 t0; t0.setZero();
// Find the SVD of E
Eigen::JacobiSVD<Mat3> svd{E, Eigen::ComputeFullU | Eigen::ComputeFullV};
Mat3 U = svd.matrixU();
Mat3 Vt = svd.matrixV().transpose();
// Find R and t
Mat3 D;
D << 0, 1, 0,
-1, 0, 0,
0, 0, 1;
Mat3 Ra = U * D * Vt;
Mat3 Rb = U * D.transpose() * Vt;
if (Ra.determinant() < 0.0) Ra *= -1.0;
if (Rb.determinant() < 0.0) Rb *= -1.0;
Vec3 tu = U.col(2);
// Figure out which configuration is correct using the supplied points
int c1_pos = 0, c2_pos = 0, c1_neg = 0, c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Ra, tu}});
const Point3 PQ = (Ra * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Ra;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Ra;
*t = -tu;
} else
{
// Triangulate again
c1_pos = c1_neg = c2_pos = c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Rb, tu}});
const Point3 PQ = (Rb * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Rb;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Rb;
*t = -tu;
} else
{
std::cerr << "[FindExtrinsics] Error: no case found!";
return false;
}
}
return true;
}
// Denormalize the results. See HZ page 109.
void UnnormalizerT::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) {
*H = T2.transpose() * (*H) * T1;
}
int main(int argc,char* argv[]) {
if (argc < 4) {
printf("./match_g2o pose_stamped.txt key.match map_point.txt\n");
return 1;
}
srand(time(NULL));
Rcl << 1,0,0,0,0,1,0,-1,0;
Tcl << 0,0.06,0;
//read pose file
FILE* pose_stamped = fopen(argv[1],"r");
if (!pose_stamped)
return 1;
char buffer[2048];
std::vector<Mat3> rotations;
std::vector<Eigen::Vector3d> translations;
while (fgets(buffer,2048,pose_stamped)) {
double t,x,y,z,qx,qy,qz,qw;
if (sscanf(buffer,"%lf %lf %lf %lf %lf %lf %lf %lf",&t,&x,&y,&z,&qw,&qx,&qy,&qz)==8) {
double r[9];
quaternionToRotation(qx,qy,qz,qw,r);
Mat3 Rwl;
memcpy(Rwl.data(),r,9*sizeof(double));
Eigen::Vector3d Twl(x,y,z);
rotations.push_back(Rcl * Rwl.transpose());
translations.push_back(- Rcl * Rwl.transpose() * Twl + Tcl);
} else {
printf("Error parsing: %s\n",buffer);
}
}
fclose(pose_stamped);
struct timespec start,end;
clock_gettime(CLOCK_MONOTONIC,&start);
int count_points = 0;
double RMSE = 0;
FILE* key_match = fopen(argv[2],"r");
FILE* map_point = fopen(argv[3],"w");
if (!(key_match && map_point))
return 1;
while (fgets(buffer,2048,key_match)) {
#if DEBUG_SINGLE
printf("key.match: %s",buffer);
#endif
int id;
char* tok = strtok(buffer," ");
std::vector<double> uc,vc;
std::vector<int> index;
while (tok) {
id = atoi(tok);
index.push_back(id);
tok = strtok(NULL," \n");
double u = atof(tok);
tok = strtok(NULL," \n");
double v = atof(tok);
tok = strtok(NULL," \n");
uc.push_back(u - cx);
vc.push_back(cy - v);
}
//optimize
Eigen::Vector3d bestEstimate, x1, x2;
double leastError = -1;
for (unsigned int i=0;i<index.size()-1;i++) {
for (unsigned int j=i+1;j<index.size();j++) {
double u1 = uc[i], v1 = vc[i];
double u2 = uc[j], v2 = vc[j];
Mat3 R1 = rotations[index[i]], R2 = rotations[index[j]];
Eigen::Vector3d T1 = translations[index[i]], T2 = translations[index[j]];
Mat3 Rcc = R2 * R1.transpose();
Eigen::Vector3d Tcc = T1 - R1 * R2.transpose() * T2;
Eigen::Vector3d r1 = Rcc.row(0), r2 = Rcc.row(1), r3 = Rcc.row(2);
Eigen::Vector3d uv(-u1/fx,-v1/fy,1);
Eigen::Vector3d mult_u = r1 + u2/fx * r3;
Eigen::Vector3d mult_v = r2 + v2/fy * r3;
double z_est[2] = {mult_u.dot(Tcc) / mult_u.dot(uv),
mult_v.dot(Tcc) / mult_v.dot(uv) } ;
for (int k=0;k<1;k++) {
x1 << -u1*z_est[k]/fx, -v1*z_est[k]/fy, z_est[k];
x2 = Rcc * (x1 - Tcc);
if (x1(2) >= 0 || x2(2) >= 0)
break;
double u_est = -fx * x2(0) / x2(2);
double v_est = -fy * x2(1) / x2(2);
double error = (u_est-u2) * (u_est-u2) + (v_est-v2) * (v_est-v2);
if (leastError < 0 || error < leastError) {
leastError = error;
bestEstimate = R1.transpose() * (x1 - T1);
}
}
}
}
//record result
RMSE += leastError;
#if DEBUG_SINGLE
printf("reprojection: ");
char* c = buffer;
c += sprintf(c,"transformation:\n");
for (unsigned int i=0;i<index.size();i++) {
id = index[i];
Eigen::Vector3d xc = rotations[id] * bestEstimate + translations[id];
double u = - fx * xc(0) / xc(2);
double v = - fy * xc(1) / xc(2);
printf("%d %f %f ",id,u+cx,cy-v);
c += sprintf(c,"%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n",
rotations[id](0,0),rotations[id](0,1),rotations[id](0,2),
rotations[id](1,0),rotations[id](1,1),rotations[id](1,2),
rotations[id](2,0),rotations[id](2,1),rotations[id](2,2));
c += sprintf(c,"[%4.2f %4.2f %4.2f]\n",translations[id](0),translations[id](1),translations[id](2));
}
printf("\n%s",buffer);
printf("estimate: %f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
#endif
fprintf(map_point,"%f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
count_points++;
}
clock_gettime(CLOCK_MONOTONIC,&end);
double dt = end.tv_sec - start.tv_sec + 0.000000001 * (end.tv_nsec - start.tv_nsec);
RMSE = sqrt(RMSE / count_points);
printf("Optimized %d map points (%fs, RMSE = %f)\n",count_points, dt, RMSE);
fclose(key_match);
fclose(map_point);
return 0;
}
