void calc_projective (const std::vector<double>& frame_ts,
const std::vector<Vec4>& gyro_quat,
const std::vector<Vec3>& acc_trans,
const std::vector<double>& gyro_ts,
CalibrationParams calib,
std::vector<Mat3>& projective)
{
int index0 = 0;
int index1 = 0;
size_t frame_count = frame_ts.size();
for (int fid = 0; fid < frame_count; fid++) {
const double ts0 = frame_ts[fid] + calib.gyro_delay;
Quatern quat0 = interp_gyro_quatern(ts0, gyro_quat, gyro_ts, index0) + Quatern(calib.gyro_drift);
const double ts1 = frame_ts[fid + 1] + calib.gyro_delay;
Quatern quat1 = interp_gyro_quatern(ts1, gyro_quat, gyro_ts, index1) + Quatern(calib.gyro_drift);
Mat3 extr0 = calc_extrinsic(quat0);
Mat3 extr1 = calc_extrinsic(quat1);
Mat3 intrinsic = calc_intrinsic(calib.fx, calib.fy, calib.cx, calib.cy, calib.skew);
Mat3 extrinsic0 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr0 * mirror_coordinate_system(AXIS_Y);
Mat3 extrinsic1 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr1 * mirror_coordinate_system(AXIS_Y);
projective[fid] = intrinsic * extrinsic0 * extrinsic1.transpose() * intrinsic.inverse();
}
}
// Camera triangulation using the iterated linear method
Vec3 Triangulation::compute(int iter) const
{
const int nviews = int(views.size());
assert(nviews>=2);
// Iterative weighted linear least squares
Mat3 AtA;
Vec3 Atb, X;
Vec weights = Vec::Constant(nviews,1.0);
for (int it=0;it<iter;++it)
{
AtA.fill(0.0);
Atb.fill(0.0);
for (int i=0;i<nviews;++i)
{
const Mat34& PMat = views[i].first;
const Vec2 & p = views[i].second;
const double w = weights[i];
Vec3 v1, v2;
for (int j=0;j<3;j++)
{
v1[j] = w * ( PMat(0,j) - p(0) * PMat(2,j) );
v2[j] = w * ( PMat(1,j) - p(1) * PMat(2,j) );
Atb[j] += w * ( v1[j] * ( p(0) * PMat(2,3) - PMat(0,3) )
+ v2[j] * ( p(1) * PMat(2,3) - PMat(1,3) ) );
}
for (int k=0;k<3;k++)
{
for (int j=0;j<=k;j++)
{
const double v = v1[j] * v1[k] + v2[j] * v2[k];
AtA(j,k) += v;
if (j<k) AtA(k,j) += v;
}
}
}
X = AtA.inverse() * Atb;
// Compute reprojection error, min and max depth, and update weights
zmin = std::numeric_limits<double>::max();
zmax = - std::numeric_limits<double>::max();
err = 0.0;
for (int i=0;i<nviews;++i)
{
const Mat34& PMat = views[i].first;
const Vec2 & p = views[i].second;
const Vec3 xProj = PMat * X.homogeneous();
const double z = xProj(2);
if (z < zmin) zmin = z;
else if (z > zmax) zmax = z;
err += (p - xProj.hnormalized()).norm(); // residual error
weights[i] = 1.0 / z;
}
}
return X;
}
Pinhole_Intrinsic(
unsigned int w = 0, unsigned int h = 0,
double focal_length_pix = 0.0,
double ppx = 0.0, double ppy = 0.0)
:IntrinsicBase(w,h)
{
_K << focal_length_pix, 0., ppx, 0., focal_length_pix, ppy, 0., 0., 1.;
_Kinv = _K.inverse();
}
ACKernelAdaptorResection_K(const Mat &x2d, const Mat &x3D, const Mat3 & K)
: x2d_(x2d.rows(), x2d.cols()), x3D_(x3D),
N1_(K.inverse()),
logalpha0_(log10(M_PI)), K_(K)
{
assert(2 == x2d_.rows());
assert(3 == x3D_.rows());
assert(x2d_.cols() == x3D_.cols());
// Normalize points by inverse(K)
ApplyTransformationToPoints(x2d, N1_, &x2d_);
}
Mat3 calc_projective (double* frame_ts,
Vec4* gyro_quat,
Vec3* acc_trans,
double* gyro_ts,
CalibrationParams calib)
{
double ts0 = frame_ts[0] + calib.gyro_delay;
Quatern rot0 = Quatern(gyro_quat[0] + calib.gyro_drift);
double ts1 = frame_ts[1] + calib.gyro_delay;
Quatern rot1 = Quatern(gyro_quat[1] + calib.gyro_drift);
Vec3 trans0 = acc_trans[0];
Vec3 trans1 = acc_trans[1];
Mat3 extr0 = calc_extrinsic(rot0);
Mat3 extr1 = calc_extrinsic(rot1);
Mat3 intrin = calc_intrinsic(calib.fx, calib.fy, calib.cx, calib.cy, calib.skew);
return intrin * extr0 * extr1.transpose() * intrin.inverse();
}
// 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 HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n) {
Mat3X x_camera_h = K.inverse() * x;
HomogeneousToEuclidean(x_camera_h, n);
}
void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n) {
Mat3X x_image_h;
EuclideanToHomogeneous(x, &x_image_h);
Mat3X x_camera_h = K.inverse() * x_image_h;
HomogeneousToEuclidean(x_camera_h, n);
}
TEST(Translation_Structure_L_Infinity_Noisy, Outlier_OSICLP_SOLVER) {
const int nViews = 5;
const int nbPoints = 5;
const double focalValue = 1000;
const double cx = 500, cy = 500;
const NViewDataSet d =
//NRealisticCamerasRing(nViews, nbPoints,
NRealisticCamerasCardioid(nViews, nbPoints,
nViewDatasetConfigurator(focalValue,focalValue,cx,cy,5,0));
d.ExportToPLY("test_Before_Infinity.ply");
//-- Test triangulation of all the point
NViewDataSet d2 = d;
//-- Set to 0 the future computed data to be sure of computation results :
d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation
fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0));
{
// Prepare Rotation matrix (premultiplied by K)
// Apply the K matrix to the rotation
Mat3 K;
K << focalValue, 0, cx,
0, focalValue, cy,
0, 0, 1;
std::vector<Mat3> vec_KRotation(nViews);
for (size_t i = 0; i < nViews; ++i) {
vec_KRotation[i] = K * d._R[i];
}
//set two point as outlier
d2._x[0].col(0)(0) +=10; //Camera 0, measurement 0, noise on X coord
d2._x[3].col(3)(1) -=8; //Camera 3, measurement 3, noise on Y coord
//Create the mega matrix
Mat megaMat(4, d._n*d._x[0].cols());
{
int cpt = 0;
for (size_t i=0; i<d._n;++i)
{
const int camIndex = i;
for (int j=0; j<d._x[0].cols(); ++j)
{
megaMat(0,cpt) = d2._x[camIndex].col(j)(0);
megaMat(1,cpt) = d2._x[camIndex].col(j)(1);
megaMat(2,cpt) = j;
megaMat(3,cpt) = camIndex;
cpt++;
}
}
}
double admissibleResidual = 1.0/focalValue;
std::vector<double> vec_solution((nViews + nbPoints + megaMat.cols())*3);
OSI_CLP_SolverWrapper wrapperOSICLPSolver(vec_solution.size());
TiXi_withNoise_L1_ConstraintBuilder cstBuilder( vec_KRotation, megaMat);
std::cout << std::endl << "Bisection returns : " << std::endl;
std::cout<< BisectionLP<TiXi_withNoise_L1_ConstraintBuilder, LP_Constraints_Sparse>(
wrapperOSICLPSolver,
cstBuilder,
&vec_solution,
admissibleResidual,
0.0, 1e-8);
std::cout << "Found solution:\n";
std::copy(vec_solution.begin(), vec_solution.end(), std::ostream_iterator<double>(std::cout, " "));
//-- First the ti and after the Xi :
//-- Fill the ti
for (int i=0; i < nViews; ++i) {
d2._t[i] = K.inverse() * Vec3(vec_solution[3*i], vec_solution[3*i+1], vec_solution[3*i+2]);
// Change Ci to -Ri*Ci
d2._C[i] = -d2._R[i].inverse() * d2._t[i];
}
for (int i=0; i < nbPoints; ++i) {
size_t index = 3*nViews;
d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]);
}
// Compute residuals L2 from estimated parameter values :
std::cout << std::endl << "Residual : " << std::endl;
Vec2 xk;
double xsum = 0.0;
for (size_t i = 0; i < d2._n; ++i) {
std::cout << "\nCamera : " << i << " \t:";
for(int k = 0; k < d._x[0].cols(); ++k)
{
xk = Project(d2.P(i), Vec3(d2._X.col(k)));
double residual = (xk - d2._x[i].col(k)).norm();
std::cout << Vec2(( xk - d2._x[i].col(k)).array().pow(2)).array().sqrt().mean() <<"\t";
//-- Check that were measurement are not noisy the residual is small
// check were the measurement are noisy, the residual is important
//if ((i != 0 && k != 0) || (i!=3 && k !=3))
if ((i != 0 && k != 0) && (i!=3 && k !=3)) {
EXPECT_NEAR(0.0, residual, 1e-5);
xsum += residual;
}
}
std::cout << std::endl;
}
double dResidual = xsum / (d2._n*d._x[0].cols());
std::cout << std::endl << "Residual mean in not noisy measurement: " << dResidual << std::endl;
// Check that 2D re-projection and 3D point are near to GT.
EXPECT_NEAR(0.0, dResidual, 1e-1);
}
d2.ExportToPLY("test_After_Infinity.ply");
}
// Denormalize the results. See HZ page 109.
void UnnormalizerI::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) {
*H = T2.inverse() * (*H) * T1;
}
