double weight_gaussian_predictive_rev(Gaussian &g1, Gaussian &g2)
{
int dim = g1.dim;
double energy1 = 0.;
Eigen::MatrixXd cov = g1.predictive_covariance + g1.predictive_covariance;
Eigen::VectorXd mean = g1.predictive_mean - g1.predictive_mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy1 += gauss;
double energy2 = 0.;
cov = g1.predictive_covariance + g2.predictive_covariance;
mean = g1.predictive_mean - g2.predictive_mean;
invij = cov.inverse();
a = mean.transpose()*invij*mean;
gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy2 += gauss;
double energy3 = 0.;
cov = g2.predictive_covariance + g2.predictive_covariance;
mean = g2.predictive_mean - g2.predictive_mean;
invij = cov.inverse();
a = mean.transpose()*invij*mean;
gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy3 += gauss;
// cout<<(maxDist - (energy1 - 2*energy2 + energy3))/maxDist<<endl;
return energy1 - 2*energy2 + energy3;
}
double weight_l2_rev(PCObject &o1, PCObject &o2)
{
double last = pcl::getTime ();
// reference :
// Robust Point Set Registration Using Gaussian Mixture Models
// Bing Jina, and Baba C. Vemuri
// IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
int n = o1.gmm.size();
int m = o2.gmm.size();
double energy1 = 0.;
for(int i=0;i<n;i++){
for(int j=0;j<n;j++){
int dim = o1.gmm.at(i).dim;
Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o1.gmm.at(j).covariance;
Eigen::VectorXd mean = o1.gmm.at(i).mean - o1.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy1 += o1.gmm.at(i).weight*o1.gmm.at(j).weight*gauss;
}
}
double energy2 = 0.;
for(int i=0;i<n;i++){
for(int j=0;j<m;j++){
int dim = o1.gmm.at(i).dim;
Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o2.gmm.at(j).covariance;
Eigen::VectorXd mean = o1.gmm.at(i).mean - o2.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy2 += o1.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
}
}
double energy3 = 0.;
for(int i=0;i<m;i++){
for(int j=0;j<m;j++){
int dim = o2.gmm.at(i).dim;
Eigen::MatrixXd cov = o2.gmm.at(i).covariance + o2.gmm.at(j).covariance;
Eigen::VectorXd mean = o2.gmm.at(i).mean - o2.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy3 += o2.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
}
}
double now = pcl::getTime ();
// cout << "l2-distance time " << now-last << " second" << endl;
// cout<<"l2distance"<<energy1 - 2*energy2 + energy3<<endl;
return (energy1 - 2*energy2 + energy3);
}
void Assembler2D::precomputeShapeFunctionDerivatives() {
Eigen::MatrixXd selector = Eigen::MatrixXd::Zero(3,2);
selector << 0.0, 0.0,
1.0, 0.0,
0.0, 1.0;
m_DN.resize(m_mesh->getNbrElements());
for (size_t i=0; i<m_mesh->getNbrElements(); ++i)
{
VectorI idx = m_mesh->getElement(i);
assert(idx.size() == 3);
VectorF u[3];
u[0] = m_mesh->getNode(idx[0]);
u[1] = m_mesh->getNode(idx[1]);
u[2] = m_mesh->getNode(idx[2]);
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(3,3);
P << 1.0, 1.0, 1.0,
u[0][0], u[1][0], u[2][0],
u[0][1], u[1][1], u[2][1];
// DN is a 4x3 matrix containing the gradients of the
// 4 shape functions (one for each node)
//
m_DN[i] = P.inverse() * selector /* * -1.0 */;
}
}
void benchmark( )
{
for (size_t i = 1; i < 12; i++)
{
const size_t N = (size_t)pow(2, i);
Eigen::MatrixXd m = MatrixXd::Random(N, N);
clock_t t0 = clock();
auto mInv = m.inverse();
clock_t t1 = clock();
double eigenT = (double)(t1-t0)/CLOCKS_PER_SEC;
t0 = clock();
auto m1 = m;
invert( m1 );
t1 = clock();
if( (m1-mInv).norm() > 1e-6 )
{
cout << "Got " << endl << m1 << endl;
cout << "Expected " << endl << mInv << endl;
cout << "Original matrix was " << endl << m << endl;
cout<< "Error : " << endl << (m1 - mInv).norm() << endl;
throw;
}
cout << N << ' ' << (double)(t1-t0) / CLOCKS_PER_SEC << ' ' << eigenT << endl;
}
}
// Convert joint torques to end effector force/torque
void computeFT(Eigen::VectorXd& ee_ft) {
Eigen::MatrixXd jac;
mRobot->getJacobian(jac);
Eigen::MatrixXd trans = jac*jac.transpose() + singular_tol*Eigen::MatrixXd::Identity(jac.rows(), jac.rows());
// f = inv(J*J')*J*tau
ee_ft = (trans.inverse()*jac)*read_torque;
}
void IEFSolver::buildIsotropicMatrix(const Cavity & cav, const IGreensFunction & gf_i, const IGreensFunction & gf_o)
{
// The total size of the cavity
size_t cavitySize = cav.size();
// The number of irreps in the group
int nrBlocks = cav.pointGroup().nrIrrep();
// The size of the irreducible portion of the cavity
int dimBlock = cav.irreducible_size();
// Compute SI and DI on the whole cavity, regardless of symmetry
Eigen::MatrixXd SI = gf_i.singleLayer(cav.elements());
Eigen::MatrixXd DI = gf_i.doubleLayer(cav.elements());
// Perform symmetry blocking
// If the group is C1 avoid symmetry blocking, we will just pack the fullPCMMatrix
// into "block diagonal" when all other manipulations are done.
if (cav.pointGroup().nrGenerators() != 0) {
symmetryBlocking(DI, cavitySize, dimBlock, nrBlocks);
symmetryBlocking(SI, cavitySize, dimBlock, nrBlocks);
}
Eigen::MatrixXd a = cav.elementArea().asDiagonal();
Eigen::MatrixXd aInv = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
aInv = a.inverse();
// Tq = -Rv -> q = -(T^-1 * R)v = -Kv
// T = (2 * M_PI * fact * aInv - DI) * a * SI; R = (2 * M_PI * aInv - DI)
// fullPCMMatrix_ = K = T^-1 * R * a
// 1. Form T
double epsilon = profiles::epsilon(gf_o.permittivity());
double fact = (epsilon + 1.0)/(epsilon - 1.0);
fullPCMMatrix_ = (2 * M_PI * fact * aInv - DI) * a * SI;
// 2. Invert T using LU decomposition with full pivoting
// This is a rank-revealing LU decomposition, this allows us
// to test if T is invertible before attempting to invert it.
Eigen::FullPivLU<Eigen::MatrixXd> T_LU(fullPCMMatrix_);
if (!(T_LU.isInvertible()))
PCMSOLVER_ERROR("T matrix is not invertible!");
fullPCMMatrix_ = T_LU.inverse();
// 3. Multiply T^-1 and R
fullPCMMatrix_ *= (2 * M_PI * aInv - DI);
// 4. Multiply by a
fullPCMMatrix_ *= a;
// 5. Symmetrize K := (K + K+)/2
if (hermitivitize_) {
hermitivitize(fullPCMMatrix_);
}
// Pack into a block diagonal matrix
// For the moment just packs into a std::vector<Eigen::MatrixXd>
symmetryPacking(blockPCMMatrix_, fullPCMMatrix_, dimBlock, nrBlocks);
std::ofstream matrixOut("PCM_matrix");
matrixOut << "fullPCMMatrix" << std::endl;
matrixOut << fullPCMMatrix_ << std::endl;
for (int i = 0; i < nrBlocks; ++i) {
matrixOut << "Block number " << i << std::endl;
matrixOut << blockPCMMatrix_[i] << std::endl;
}
built_ = true;
}
void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(
shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
const Eigen::MatrixXd& H_delayed, const Eigen::MatrixXd & res_delayed,
const Eigen::MatrixXd& R_delayed) {
// Get measurements.
/// Correction from EKF update.
Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;
Eigen::MatrixXd S;
Eigen::MatrixXd K(
static_cast<int>(MSF_Core<EKFState_T>::nErrorStatesAtCompileTime),
R_delayed.rows());
typename MSF_Core<EKFState_T>::ErrorStateCov & P = state->P;
S = H_delayed * P * H_delayed.transpose() + R_delayed;
K = P * H_delayed.transpose() * S.inverse();
correction_ = K * res_delayed;
const typename MSF_Core<EKFState_T>::ErrorStateCov KH =
(MSF_Core<EKFState_T>::ErrorStateCov::Identity() - K * H_delayed);
P = KH * P * KH.transpose() + K * R_delayed * K.transpose();
// Make sure P stays symmetric.
P = 0.5 * (P + P.transpose());
core.applyCorrection(state, correction_);
}
double* simulatePlant(double *state_vect, double *input_vect, double *state_op_vect, double sampling_time)
{
double input_op_vect[4] = {0.0, 0.0, 0.0, 0.0};
Eigen::Map<Eigen::VectorXd> x(state_vect, num_states_);
Eigen::Map<Eigen::VectorXd> x_bar(state_op_vect, num_states_);
Eigen::Map<Eigen::VectorXd> u(input_vect, num_inputs_);
Eigen::Map<Eigen::VectorXd> u_bar(&input_op_vect[0], num_inputs_);
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(num_states_, num_states_);
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(num_states_, num_inputs_);
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(num_inputs_, num_inputs_);
Eigen::VectorXd f_bar = Eigen::MatrixXd::Zero(num_inputs_, 1);
double phi = x_bar(6);
double theta = x_bar(7);
double p = x_bar(9);
double q = x_bar(10);
double r = x_bar(11);
M << 1., 1., 1., 1., 0., -1., 0., 1., 1., 0., -1., 0., -1., 1., -1., 1.;
f_bar(0) = g_ * m_ / (Ct_ * cos(phi) * cos(theta));
f_bar(1) = (Izz_ - Iyy_) * q * r / (Ct_ * d_);
f_bar(2) = (Izz_ - Ixx_) * p * r / (Ct_ * d_);
f_bar(3) = (Iyy_ - Ixx_) * p * q / Cq_;
u_bar = M.inverse() * f_bar;
u_bar = u_bar.cwiseSqrt();
// Compute the matrices of the linear dynamic model
computeLTIModel(A, B, state_op_vect, input_op_vect);
x = x_bar + A * (x - x_bar) + B * (u - u_bar);
return state_vect;
}
void IEFSolver::buildAnisotropicMatrix(const Cavity & cav, const IGreensFunction & gf_i, const IGreensFunction & gf_o)
{
// The total size of the cavity
size_t cavitySize = cav.size();
// The number of irreps in the group
int nrBlocks = cav.pointGroup().nrIrrep();
// The size of the irreducible portion of the cavity
int dimBlock = cav.irreducible_size();
// Compute SI, DI and SE, DE on the whole cavity, regardless of symmetry
Eigen::MatrixXd SI = gf_i.singleLayer(cav.elements());
Eigen::MatrixXd DI = gf_i.doubleLayer(cav.elements());
Eigen::MatrixXd SE = gf_o.singleLayer(cav.elements());
Eigen::MatrixXd DE = gf_o.doubleLayer(cav.elements());
// Perform symmetry blocking
// If the group is C1 avoid symmetry blocking, we will just pack the fullPCMMatrix
// into "block diagonal" when all other manipulations are done.
if (cav.pointGroup().nrGenerators() != 0) {
symmetryBlocking(DI, cavitySize, dimBlock, nrBlocks);
symmetryBlocking(SI, cavitySize, dimBlock, nrBlocks);
symmetryBlocking(DE, cavitySize, dimBlock, nrBlocks);
symmetryBlocking(SE, cavitySize, dimBlock, nrBlocks);
}
Eigen::MatrixXd a = cav.elementArea().asDiagonal();
Eigen::MatrixXd aInv = a.inverse();
// 1. Form T
fullPCMMatrix_ = ((2 * M_PI * aInv - DE) * a * SI + SE * a * (2 * M_PI * aInv + DI.adjoint().eval()));
// 2. Invert T using LU decomposition with full pivoting
// This is a rank-revealing LU decomposition, this allows us
// to test if T is invertible before attempting to invert it.
Eigen::FullPivLU<Eigen::MatrixXd> T_LU(fullPCMMatrix_);
if (!(T_LU.isInvertible())) PCMSOLVER_ERROR("T matrix is not invertible!");
fullPCMMatrix_ = T_LU.inverse();
Eigen::FullPivLU<Eigen::MatrixXd> SI_LU(SI);
if (!(SI_LU.isInvertible())) PCMSOLVER_ERROR("SI matrix is not invertible!");
fullPCMMatrix_ *= ((2 * M_PI * aInv - DE) - SE * SI_LU.inverse() * (2 * M_PI * aInv - DI));
fullPCMMatrix_ *= a;
// 5. Symmetrize K := (K + K+)/2
if (hermitivitize_) {
hermitivitize(fullPCMMatrix_);
}
// Pack into a block diagonal matrix
// For the moment just packs into a std::vector<Eigen::MatrixXd>
symmetryPacking(blockPCMMatrix_, fullPCMMatrix_, dimBlock, nrBlocks);
std::ofstream matrixOut("PCM_matrix");
matrixOut << "fullPCMMatrix" << std::endl;
matrixOut << fullPCMMatrix_ << std::endl;
for (int i = 0; i < nrBlocks; ++i) {
matrixOut << "Block number " << i << std::endl;
matrixOut << blockPCMMatrix_[i] << std::endl;
}
built_ = true;
}
void calc()
{
Eigen::MatrixXd M_inv1 = M1.inverse();
Eigen::MatrixXd M_inv2 = M2.inverse();
Eigen::MatrixXd Jv1 = J1.block(0, 0, 3, J1.cols());
Eigen::MatrixXd Jv2 = J2.block(0, 0, 3, J2.cols());
Eigen::MatrixXd Lambda_inv1 = Jv1 * M_inv1 * Jv1.transpose();
Eigen::MatrixXd Lambda_inv2 = Jv2 * M_inv2 * Jv2.transpose();
Eigen::MatrixXd Lambda1 = Lambda_inv1.inverse();
Eigen::MatrixXd Lambda2 = Lambda_inv2.inverse();
Eigen::MatrixXd J_dyn_inv1 = M_inv1 * Jv1.transpose() * Lambda1;
Eigen::MatrixXd J_dyn_inv2 = M_inv2 * Jv2.transpose() * Lambda2;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(M_inv1.rows(), M_inv1.rows());
Eigen::MatrixXd N1 = I - J_dyn_inv1 * Jv1;
Eigen::MatrixXd N2 = I - J_dyn_inv2 * Jv2;
//std::cout << N.transpose() << std::endl << std::endl;
}
void PseudoInverse(const ::Eigen::MatrixXd& inputMatrix, ::Eigen::MatrixXd& outputMatrix, const ::Eigen::MatrixXd& weight)
{
Eigen::MatrixXd tmp;
if (weight.size() > 0)
tmp = inputMatrix.transpose() * weight.transpose() * weight * inputMatrix;
else
tmp = inputMatrix * inputMatrix.transpose();
tmp = tmp.inverse();
outputMatrix = inputMatrix.transpose() * tmp;
}
double weight_l2_gauss(PCObject &o1, PCObject &o2)
{
// l2 distance
Eigen::MatrixXd covsum = o1.gaussian.covariance+o2.gaussian.covariance;
Eigen::VectorXd meandiff = o1.gaussian.mean - o2.gaussian.mean;
Eigen::MatrixXd inv = covsum.inverse();
double det = covsum.determinant();
double a = meandiff.transpose()*inv*meandiff;
double l2 = 2.-2.*(1./sqrt(pow(2*pi,3)*det)) * exp(-0.5*a);
if(l2 < 0) l2 = 0.;
return l2;
}
double multivariateGaussianDensity(const Eigen::VectorXd& mean,
const Eigen::MatrixXd& cov,
const Eigen::VectorXd& z)
{
Eigen::VectorXd diff = mean - z;
Eigen::VectorXd exponent = -0.5 * (diff.transpose() * cov.inverse() * diff);
return pow(2 * M_PI, (double) z.size() / -2.0) * pow(cov.determinant(), -0.5) *
exp(exponent(0));
}
void calc()
{
//std::cout << "M : " << std::endl << M.inverse() << std::endl;
// << "J : " << std::endl << J << std::endl << std::endl;
Eigen::MatrixXd M_inv = M.inverse();
Eigen::MatrixXd Jv = J.block(0, 0, 3, J.cols());
Eigen::MatrixXd Lambda_inv = Jv * M_inv * Jv.transpose();
Eigen::MatrixXd Lambda = Lambda_inv.inverse();
Eigen::MatrixXd J_dyn_inv = M_inv * Jv.transpose() * Lambda;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(15, 15);
Eigen::MatrixXd N = I - J_dyn_inv * Jv;
//std::cout << N.transpose() << std::endl << std::endl;
}
/**
* @function Residual
*/
void Residual( Eigen::MatrixXd _M, std::vector<Eigen::VectorXi> _points2D,
std::vector<Eigen::VectorXd> _points3D,
std::vector<Eigen::VectorXd> _normPoints2D,
std::vector<Eigen::VectorXd> _normPoints3D,
Eigen::MatrixXd T2 )
{
int N = _points2D.size();
Eigen::VectorXd uvh;
double u; double v;
std::vector<Eigen::VectorXd> uv_normPredicted;
std::vector<Eigen::VectorXd> uv_predicted;
std::vector<Eigen::VectorXd> normResidual;
std::vector<Eigen::VectorXd> residual;
Eigen::MatrixXd T2inv;
for( int i = 0; i < N; i++ )
{
Eigen::VectorXd normPoint3D_h(4);
normPoint3D_h << _normPoints3D[i](0), _normPoints3D[i](1), _normPoints3D[i](2), 1;
uvh = _M*normPoint3D_h;
u = uvh(0) / uvh(2); v = uvh(1) / uvh(2);
uv_normPredicted.push_back( Eigen::Vector2d( u, v ) );
double nru = u - _normPoints2D[i](0);
double nrv = v - _normPoints2D[i](1);
double nrd = sqrt( nru*nru + nrv*nrv );
normResidual.push_back( Eigen::Vector3d( nru, nrv, nrd) );
T2inv = T2.inverse();
Eigen::VectorXd uvp = T2inv*Eigen::Vector3d( u, v, 1 );
double ru = uvp(0) - _points2D[i](0);
double rv = uvp(1) - _points2D[i](1);
double rd = sqrt( ru*ru + rv*rv );
uv_predicted.push_back( Eigen::Vector2d( uvp(0), uvp(1) ) );
residual.push_back( Eigen::Vector3d( ru, rv, rd ) );
}
/** Display */
printf("Residual!! \n");
for( unsigned int i = 0; i < N; i++ )
{
printf("[%d] P2D: (%d , %d) -- Predicted: (%.4f , %.4f) - Residual: (%.4f %.4f) Mod: %.4f \n",i, _points2D[i](0), _points2D[i](1), uv_predicted[i](0), uv_predicted[i](1), residual[i](0), residual[i](1), residual[i](2));
}
}
CCircle CMinSquareRecognizing::getCircleByPoints( std::vector<cv::Point> points )
{
Eigen::MatrixXd C( 4, 4 );
C.fill( 0 );
C( 0, 0 ) = 4.;
Eigen::MatrixXd S = generateScatterMatrix( points );
Eigen::MatrixXd InverseS = S.inverse();
Eigen::MatrixXd engineM = InverseS * C;
Eigen::EigenSolver<Eigen::MatrixXd> es( engineM, true );
Eigen::MatrixXd vecs = es.eigenvectors().real();
Eigen::MatrixXd vals = es.eigenvalues().real();
for( int i = 0; i < vecs.rows(); ++i ) {
Eigen::MatrixXd row = vecs.row( i );
Eigen::MatrixXd rowTranspose = row.transpose();
double tmp = ( row * S * rowTranspose )( 0, 0 );
if( ( 1. / vals( i, 0 ) ) > 0
&& !isAbsInf( 1. / vals( i, 0 ) )
&& !isAbsInf( vals( i, 0 ) )
&& tmp > 0 ) {
//нормируем коэффициент перед квадратами до 1
double a = vecs( 0, i );
double b = vecs( 1, i ) / a;
double c = vecs( 2, i ) / a;
double d = vecs( 3, i ) / a;
double x = -b / 2.;
double y = -c / 2.;
double r = sqrt( b * b + c * c - 4 * d ) / 2.;
double accuracy = countCircleAccuracy( x, y, r, points );
return CCircle( cvRound( x ), cvRound( y ), cvRound( r ), accuracy );
}
}
return CCircle( 0, 0, 0 );
}
/**
* @function main
*/
int main( int argc, char* argv[] )
{
if( argc < 3 )
{
printf("Error. Give me two files with 2D and 3D Points \n");
return 1;
}
/** Normalize */
normalizePoints2D( argv[1], T2a, points2Da, normPoints2Da );
normalizePoints2D( argv[2], T2b, points2Db, normPoints2Db );
/** SVD */
calculateF_SVD( normPoints2Da, normPoints2Db, Fh );
/** Print Fh */
std::cout << Fh << std::endl;
std::cout << Fh.inverse() << std::endl;
return 0;
}
// Eigen version
virtual ReturnValue evaluate()
{
CPad* pad = getPad("SqMatrixPad");
SqMatrix* sqMatrix = (SqMatrix*) pad->getRootObject(sizeof(SqMatrix));
if (sqMatrix == NULL)
throwUdxException("Pad does not exist.");
Eigen::MatrixXd m = Eigen::Map<RMatrixXd>(
sqMatrix->matrix,
sqMatrix->dimension,
sqMatrix->dimension);
RMatrixXd i = m.inverse();
Eigen::Map<RMatrixXd>(
sqMatrix->matrix,
sqMatrix->dimension,
sqMatrix->dimension) = i;
NZ_UDX_RETURN_BOOL(true);
}
void mexFunction (int numOutputs, mxArray *outputs[],
int numInputs, const mxArray *inputs[])
{
if(numInputs == 3){
//Eigen::initParallel();
ConstMatrix<double, Eigen::Dynamic, Eigen::Dynamic> X(inputs[0]);
// Get size of data.
int samples = X.rows();
int dimension = X.cols();
if (dimension<1){
mexErrMsgTxt("stats:mvnpdf:TooFewDimensions");
}
ConstMatrix<double, 1, Eigen::Dynamic> mu(inputs[1]);
ConstMatrix<double, Eigen::Dynamic, Eigen::Dynamic> C(inputs[2]);
if (mu.cols()!=dimension || C.rows()!=dimension || C.cols()!=dimension ){
mexErrMsgTxt("stats:mvnpdf:invalidparameters");
}
outputs[0] = mxCreateDoubleMatrix (samples, 1, mxREAL);
double* result = mxGetPr(outputs[0]);
double lognormconst;
switch(dimension){
case 1:
{
lognormconst = log(1.0/sqrt(2 * M_PI*C(0,0)));
double factor = -0.5 /C(0,0);
//#pragma omp parallel for
for (int i=0; i<samples; i++){
double x = X(i,0) - mu(0,0);
double exponent =x*x*factor;
result[i] = lognormconst + exponent;
}
fmath::expd_v(result, X.rows());
}
break;
case 2:
calculateGaussPdf<2>(X, mu, C, result);
break;
case 3:
calculateGaussPdf<3>(X, mu, C, result);
break;
case 4:
calculateGaussPdf<4>(X, mu, C, result);
break;
/*
case 5:
calculateGaussPdf<5>(X, mu, C, result);
break;
case 6:
calculateGaussPdf<6>(X, mu, C, result);
break;
case 7:
calculateGaussPdf<7>(X, mu, C, result);
break; */
default:
//calculateGaussPdf<Eigen::Dynamic>(X, mu, C, result);
//break;
Eigen::MatrixXd L = C.llt().matrixL().transpose(); // cholesky decomposition
Eigen::MatrixXd Linv = L.inverse();
double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C
lognormconst = -log(2 * M_PI)*dimension/2 - log(fabs(det));
Eigen::MatrixXd X0 = (X.rowwise() - mu)*Linv;
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, samples);
resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst;
/*
Eigen::Matrix<double,1,Eigen::Dynamic> x = mu;
Eigen::Matrix<double,1,Eigen::Dynamic> tmp = x;
for (int i=0; i< samples; i++){
x = X.row(i) - mu;
tmp.noalias() = x*Linv;
double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum();
result[i] = lognormconst+exponent;
}*/
fmath::expd_v(result, X.rows());
break;
}
}
else{
mexErrMsgTxt("stats:mvnpdf:invalidparameters");
}
}
// left right back front
// constantCoord => 0: (-0.5, _, _), 1: (0.5, _, _), 2: (_, -0.5, _), 3: (_, 0.5, _)
// also include the minimum and maximum values in the non-constant coord (mn, mx)
// and the minimum and maximum values for this coord (o_mn, o_mx)
void SliceStack::triangulateSide(int constantCoord,
double fixedCoord,
vector<Eigen::Vector3d> &verts,
Eigen::MatrixXd &V, Eigen::MatrixXi &F) {
// Compute the mid z-coord.
sort(verts.begin(), verts.end(), customSortByZ);
double mid = 0;
for (const auto & v : verts) {
mid += v(2);
}
mid /= verts.size();
vector<Eigen::Vector3d> topV;
vector<Eigen::Vector3d> botV;
// Fill the botV and topV vectors
for (int i = 0; i < verts.size(); ++i) {
if (verts[i][2] < mid)
botV.push_back(verts[i]);
else
topV.push_back(verts[i]);
}
// Set the z-scaling as the distance between the top and the bottom vert
double z_spacing = verts.back()(2) - verts.front()(2);
// If the constant coord is y, sort by x direction.
if (constantCoord == GLOBAL::FRONT || constantCoord == GLOBAL::BACK) {
sort(botV.begin(), botV.end(), customSortByX);
sort(topV.rbegin(), topV.rend(), customSortByX); // backwards
}
// otherwise, sort by y direction.
else {
sort(botV.begin(), botV.end(), customSortByY);
sort(topV.rbegin(), topV.rend(), customSortByY); // backwards
}
Eigen::MatrixXd inputV(verts.size() + GLOBAL::EXTRA * 2, 2);
Eigen::MatrixXi inputE(verts.size() + GLOBAL::EXTRA * 2, 2);
int offset = 0;
// Add bottom points, from left to right
for (int i = 0; i < botV.size(); ++i) {
if (constantCoord == GLOBAL::LEFT || constantCoord == GLOBAL::RIGHT) {
inputV.row(offset++) << botV[i][1], botV[i][2];
} else {
inputV.row(offset++) << botV[i][0], botV[i][2];
}
}
// Add right points
// Get the vector that extends from the top corner to the bottom corner.
Eigen::Vector3d bot2Top = topV.front() - botV.back(); // top is sorted backwards
for (int i = 1; i <= GLOBAL::EXTRA; ++i) {
auto shift_by = botV.back() + bot2Top * i / (GLOBAL::EXTRA + 1);
if (constantCoord == GLOBAL::LEFT || constantCoord == GLOBAL::RIGHT)
inputV.row(offset++) << shift_by(1), shift_by(2);
else
inputV.row(offset++) << shift_by(0), shift_by(2);
}
// Add top points, from right to left
for (int i = 0; i < topV.size(); ++i) {
if (constantCoord == GLOBAL::LEFT || constantCoord == GLOBAL::RIGHT)
inputV.row(offset++) << topV[i][1], topV[i][2];
else
inputV.row(offset++) << topV[i][0], topV[i][2];
}
// Add left points
Eigen::Vector3d top2Bot = botV.front() - topV.back();
for (int i = 1; i <= GLOBAL::EXTRA; ++i) {
auto shift_by = topV.back() + top2Bot * i / (GLOBAL::EXTRA + 1);
if (constantCoord == GLOBAL::LEFT || constantCoord == GLOBAL::RIGHT)
inputV.row(offset++) << shift_by(1), shift_by(2);
else
inputV.row(offset++) << shift_by(0), shift_by(2);
}
// Add all the new edges.
for (int i = 0; i < inputV.rows(); ++i) {
inputE.row(i) << i, (i + 1) % (inputV.rows());
}
Eigen::MatrixXd tmpV;
Helpers::triangulate(inputV, inputE, tmpV, F);
// 3D-ifying the slice.
V.resize(tmpV.rows(), 3);
for (int i = 0; i < tmpV.rows(); ++i) {
switch(constantCoord) {
case GLOBAL::LEFT : V.row(i) << fixedCoord, tmpV(i, 0), tmpV(i, 1);
break;
case GLOBAL::RIGHT : V.row(i) << fixedCoord, tmpV(i, 0), tmpV(i, 1);
break;
case GLOBAL::FRONT : V.row(i) << tmpV(i, 0), fixedCoord, tmpV(i, 1);
break;
case GLOBAL::BACK : V.row(i) << tmpV(i, 0), fixedCoord, tmpV(i, 1);
break;
}
}
// Find the projection to map the corners back.
Eigen::MatrixXd XT(3, 3);
XT << V.row(0),
V.row(botV.size() - 1),
V.row(botV.size() + GLOBAL::EXTRA);
Eigen::MatrixXd X = XT.transpose();
Eigen::MatrixXd B(3, 3);
B.col(0) << botV[0];
B.col(1) << botV[botV.size()-1];
B.col(2) << topV[0];
Eigen::MatrixXd A = B * X.inverse();
V = (A * V.transpose()).transpose();
}
int main(int argc, char *argv[])
{
TCLAP::CmdLine cmd("LINE3D++");
TCLAP::ValueArg<std::string> inputArg("i", "input_folder", "folder containing the images", true, ".", "string");
cmd.add(inputArg);
TCLAP::ValueArg<std::string> mavmapArg("b", "mavmap_output", "full path to the mavmap output (image-data-*.txt)", true, "", "string");
cmd.add(mavmapArg);
TCLAP::ValueArg<std::string> extArg("t", "image_extension", "image extension (case sensitive), if not specified: jpg, png or bmp expected", false, "", "string");
cmd.add(extArg);
TCLAP::ValueArg<std::string> prefixArg("f", "image_prefix", "optional image prefix", false, "", "string");
cmd.add(prefixArg);
TCLAP::ValueArg<std::string> outputArg("o", "output_folder", "folder where result and temporary files are stored (if not specified --> input_folder+'/Line3D++/')", false, "", "string");
cmd.add(outputArg);
TCLAP::ValueArg<int> scaleArg("w", "max_image_width", "scale image down to fixed max width for line segment detection", false, L3D_DEF_MAX_IMG_WIDTH, "int");
cmd.add(scaleArg);
TCLAP::ValueArg<int> neighborArg("n", "num_matching_neighbors", "number of neighbors for matching", false, L3D_DEF_MATCHING_NEIGHBORS, "int");
cmd.add(neighborArg);
TCLAP::ValueArg<float> sigma_A_Arg("a", "sigma_a", "angle regularizer", false, L3D_DEF_SCORING_ANG_REGULARIZER, "float");
cmd.add(sigma_A_Arg);
TCLAP::ValueArg<float> sigma_P_Arg("p", "sigma_p", "position regularizer (if negative: fixed sigma_p in world-coordinates)", false, L3D_DEF_SCORING_POS_REGULARIZER, "float");
cmd.add(sigma_P_Arg);
TCLAP::ValueArg<float> epipolarArg("e", "min_epipolar_overlap", "minimum epipolar overlap for matching", false, L3D_DEF_EPIPOLAR_OVERLAP, "float");
cmd.add(epipolarArg);
TCLAP::ValueArg<int> knnArg("k", "knn_matches", "number of matches to be kept (<= 0 --> use all that fulfill overlap)", false, L3D_DEF_KNN, "int");
cmd.add(knnArg);
TCLAP::ValueArg<int> segNumArg("y", "num_segments_per_image", "maximum number of 2D segments per image (longest)", false, L3D_DEF_MAX_NUM_SEGMENTS, "int");
cmd.add(segNumArg);
TCLAP::ValueArg<int> visibilityArg("v", "visibility_t", "minimum number of cameras to see a valid 3D line", false, L3D_DEF_MIN_VISIBILITY_T, "int");
cmd.add(visibilityArg);
TCLAP::ValueArg<bool> diffusionArg("d", "diffusion", "perform Replicator Dynamics Diffusion before clustering", false, L3D_DEF_PERFORM_RDD, "bool");
cmd.add(diffusionArg);
TCLAP::ValueArg<bool> loadArg("l", "load_and_store_flag", "load/store segments (recommended for big images)", false, L3D_DEF_LOAD_AND_STORE_SEGMENTS, "bool");
cmd.add(loadArg);
TCLAP::ValueArg<float> collinArg("r", "collinearity_t", "threshold for collinearity", false, L3D_DEF_COLLINEARITY_T, "float");
cmd.add(collinArg);
TCLAP::ValueArg<bool> cudaArg("g", "use_cuda", "use the GPU (CUDA)", false, true, "bool");
cmd.add(cudaArg);
TCLAP::ValueArg<bool> ceresArg("c", "use_ceres", "use CERES (for 3D line optimization)", false, L3D_DEF_USE_CERES, "bool");
cmd.add(ceresArg);
TCLAP::ValueArg<float> constRegDepthArg("z", "const_reg_depth", "use a constant regularization depth (only when sigma_p is metric!)", false, -1.0f, "float");
cmd.add(constRegDepthArg);
// read arguments
cmd.parse(argc,argv);
std::string inputFolder = inputArg.getValue().c_str();
std::string mavmapFile = mavmapArg.getValue().c_str();
std::string outputFolder = outputArg.getValue().c_str();
std::string imgExtension = extArg.getValue().c_str();
std::string imgPrefix = prefixArg.getValue().c_str();
if(outputFolder.length() == 0)
outputFolder = inputFolder+"/Line3D++/";
int maxWidth = scaleArg.getValue();
unsigned int neighbors = std::max(neighborArg.getValue(),2);
bool diffusion = diffusionArg.getValue();
bool loadAndStore = loadArg.getValue();
float collinearity = collinArg.getValue();
bool useGPU = cudaArg.getValue();
bool useCERES = ceresArg.getValue();
float epipolarOverlap = fmin(fabs(epipolarArg.getValue()),0.99f);
float sigmaA = fabs(sigma_A_Arg.getValue());
float sigmaP = sigma_P_Arg.getValue();
int kNN = knnArg.getValue();
unsigned int maxNumSegments = segNumArg.getValue();
unsigned int visibility_t = visibilityArg.getValue();
float constRegDepth = constRegDepthArg.getValue();
if(imgExtension.substr(0,1) != ".")
imgExtension = "."+imgExtension;
// since no median depth can be computed without camera-to-worldpoint links
// sigma_p MUST be positive and in pixels!
if(sigmaP < L3D_EPS && constRegDepth < L3D_EPS)
{
std::cout << "sigma_p cannot be negative (i.e. in world coordiantes) when no valid regularization depth (--const_reg_depth) is given!" << std::endl;
std::cout << "reverting to: sigma_p = 2.5px" << std::endl;
sigmaP = 2.5f;
}
// check if mavmap file exists
boost::filesystem::path bf(mavmapFile);
if(!boost::filesystem::exists(bf))
{
std::cerr << "mavmap file '" << mavmapFile << "' does not exist!" << std::endl;
return -1;
}
// create output directory
boost::filesystem::path dir(outputFolder);
boost::filesystem::create_directory(dir);
// create Line3D++ object
L3DPP::Line3D* Line3D = new L3DPP::Line3D(outputFolder,loadAndStore,maxWidth,
maxNumSegments,false,useGPU);
// read mavmap result
std::ifstream mavmap_file;
mavmap_file.open(mavmapFile.c_str());
std::string mavmap_line;
// check for comments...
while(std::getline(mavmap_file,mavmap_line))
{
if(mavmap_line.substr(0,1) != "#")
break;
}
// read camera data (sequentially)
std::vector<std::string> cams_filenames;
std::vector<std::pair<double,double> > cams_focals;
std::vector<Eigen::Matrix3d> cams_rotation;
std::vector<Eigen::Vector3d> cams_translation;
std::vector<Eigen::Vector2d> cams_principle;
do
{
if(mavmap_line.length() < 28)
break;
std::string filename,roll,pitch,yaw;
std::string lat,lon,alt,h;
std::string tx,ty,tz;
std::string camID,camModel,fx,fy,cx,cy;
std::stringstream mavmap_stream(mavmap_line);
mavmap_stream >> filename >> roll >> pitch >> yaw;
mavmap_stream >> lat >> lon >> alt >> h;
mavmap_stream >> tx >> ty >> tz;
mavmap_stream >> camID >> camModel >> fx >> fy >> cx >> cy;
// check camera model
if(camModel.substr(0,camModel.length()-1) != "PINHOLE")
{
std::cout << "only PINHOLE camera model supported..." << std::endl;
return -1;
}
// filename
cams_filenames.push_back(filename.substr(0,filename.length()-1));
// rotation
double r,y,p;
std::stringstream dat_stream(roll.substr(0,roll.length()-1));
dat_stream >> r; dat_stream.str(""); dat_stream.clear();
dat_stream.str(pitch.substr(0,pitch.length()-1));
dat_stream >> p; dat_stream.str(""); dat_stream.clear();
dat_stream.str(yaw.substr(0,yaw.length()-1));
dat_stream >> y; dat_stream.str(""); dat_stream.clear();
Eigen::Matrix3d R = Line3D->rotationFromRPY(r,p,y);
// translation
double Tx,Ty,Tz;
dat_stream.str(tx.substr(0,tx.length()-1));
dat_stream >> Tx; dat_stream.str(""); dat_stream.clear();
dat_stream.str(ty.substr(0,ty.length()-1));
dat_stream >> Ty; dat_stream.str(""); dat_stream.clear();
dat_stream.str(tz.substr(0,tz.length()-1));
dat_stream >> Tz; dat_stream.str(""); dat_stream.clear();
Eigen::Vector3d t(Tx,Ty,Tz);
// invert
Eigen::MatrixXd Rt = Eigen::MatrixXd::Identity(4,4);
Rt.block<3,3>(0,0) = R;
Rt.block<3,1>(0,3) = t;
Eigen::MatrixXd Rt_inv = Rt.inverse().eval().block<3, 4>(0,0);
R = Rt_inv.block<3,3>(0,0);
t = Rt_inv.block<3,1>(0,3);
cams_rotation.push_back(R);
cams_translation.push_back(t);
// focal lengths
double foc_x,foc_y;
dat_stream.str(fx.substr(0,fx.length()-1));
dat_stream >> foc_x; dat_stream.str(""); dat_stream.clear();
dat_stream.str(fy.substr(0,fy.length()-1));
dat_stream >> foc_y; dat_stream.str(""); dat_stream.clear();
cams_focals.push_back(std::pair<double,double>(foc_x,foc_y));
// principle point
double pp_x,pp_y;
dat_stream.str(cx.substr(0,cx.length()-1));
dat_stream >> pp_x; dat_stream.str(""); dat_stream.clear();
dat_stream.str(cy.substr(0,cy.length()-1));
dat_stream >> pp_y; dat_stream.str(""); dat_stream.clear();
cams_principle.push_back(Eigen::Vector2d(pp_x,pp_y));
}while(std::getline(mavmap_file,mavmap_line));
mavmap_file.close();
// load images (parallel)
#ifdef L3DPP_OPENMP
#pragma omp parallel for
#endif //L3DPP_OPENMP
for(unsigned int i=0; i<cams_rotation.size(); ++i)
{
// load image
std::string img_filename = imgPrefix+cams_filenames[i];
cv::Mat image;
std::vector<boost::filesystem::wpath> possible_imgs;
if(imgExtension.length() == 0)
{
// look for common image extensions
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".jpg"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".JPG"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".png"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".PNG"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".jpeg"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".JPEG"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".bmp"));
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+".BMP"));
}
else
{
// use given extension
possible_imgs.push_back(boost::filesystem::wpath(inputFolder+"/"+img_filename+imgExtension));
}
bool image_found = false;
unsigned int pos = 0;
while(!image_found && pos < possible_imgs.size())
{
if(boost::filesystem::exists(possible_imgs[pos]))
{
image_found = true;
img_filename = possible_imgs[pos].string();
}
++pos;
}
if(image_found)
{
// load image
image = cv::imread(img_filename,CV_LOAD_IMAGE_GRAYSCALE);
// setup intrinsics
double px = cams_principle[i].x();
double py = cams_principle[i].y();
double fx = cams_focals[i].first;
double fy = cams_focals[i].second;
Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
K(0,0) = fx;
K(1,1) = fy;
K(0,2) = px;
K(1,2) = py;
K(2,2) = 1.0;
// set neighbors
std::list<unsigned int> neighbor_list;
size_t n_before = neighbors/2;
for(int nID=int(i)-1; nID >= 0 && neighbor_list.size()<n_before; --nID)
{
neighbor_list.push_back(nID);
}
for(int nID=int(i)+1; nID < int(cams_rotation.size()) && neighbor_list.size() < neighbors; ++nID)
{
neighbor_list.push_back(nID);
}
// add to system
Line3D->addImage(i,image,K,cams_rotation[i],
cams_translation[i],constRegDepth,neighbor_list);
}
}
// match images
Line3D->matchImages(sigmaP,sigmaA,neighbors,epipolarOverlap,
kNN,constRegDepth);
// compute result
Line3D->reconstruct3Dlines(visibility_t,diffusion,collinearity,useCERES);
// save end result
std::vector<L3DPP::FinalLine3D> result;
Line3D->get3Dlines(result);
// save as STL
Line3D->saveResultAsSTL(outputFolder);
// save as OBJ
Line3D->saveResultAsOBJ(outputFolder);
// save as TXT
Line3D->save3DLinesAsTXT(outputFolder);
// save as BIN
Line3D->save3DLinesAsBIN(outputFolder);
// cleanup
delete Line3D;
}
pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const
{
double gu = u;
double gv = v;
double gw = 0;
MLSProjectionResults result;
result.normal = plane_normal;
if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
{
PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
gw = d.z;
double err_total;
double dist1 = std::abs (gw - w);
double dist2;
do
{
double e1 = (gu - u) + d.z_u * gw - d.z_u * w;
double e2 = (gv - v) + d.z_v * gw - d.z_v * w;
double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w;
double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w;
double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w;
double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w;
Eigen::MatrixXd J (2, 2);
J (0, 0) = F1u;
J (0, 1) = F1v;
J (1, 0) = F2u;
J (1, 1) = F2v;
Eigen::Vector2d err (e1, e2);
Eigen::Vector2d update = J.inverse () * err;
gu -= update (0);
gv -= update (1);
d = getPolynomialPartialDerivative (gu, gv);
gw = d.z;
dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));
err_total = std::sqrt (e1 * e1 + e2 * e2);
} while (err_total > 1e-8 && dist2 < dist1);
if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection
{
gu = u;
gv = v;
d = getPolynomialPartialDerivative (u, v);
gw = d.z;
}
result.u = gu;
result.v = gv;
result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
result.normal.normalize ();
}
result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;
return (result);
}
// another simple example: invert a matrix,
// returning a matrix
//
// [[Rcpp::export]]
Eigen::MatrixXd rcppeigen_matinv(const Eigen::MatrixXd & x) {
Eigen::MatrixXd Xinv(x.inverse());
return Xinv;
}
TEST(SparseMatrixFunctionTests, testSchurComplement1)
{
try {
using namespace aslam::backend;
typedef sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> SparseBlockMatrix;
// Create the sparse Hessian. Two dense blocks. Three sparse.
int structure[5] = {2, 2, 3, 3, 3};
std::partial_sum(structure, structure + 5, structure);
int marginalizedStartingBlock = 2;
int marginalizedStartingIndex = structure[ marginalizedStartingBlock - 1 ];
double lambda = 1;
SparseBlockMatrix H(structure, structure, 5, 5, true);
Eigen::VectorXd e(H.rows());
e.setRandom();
Eigen::VectorXd b(H.rowBaseOfBlock(marginalizedStartingBlock));
b.setZero();
boost::shared_ptr<SparseBlockMatrix> A(H.slice(0, marginalizedStartingBlock, 0, marginalizedStartingBlock, true));
ASSERT_EQ(marginalizedStartingBlock, A->bRows());
ASSERT_EQ(marginalizedStartingBlock, A->bCols());
A->clear(false);
std::vector<Eigen::MatrixXd> invVi;
invVi.resize(H.bRows() - marginalizedStartingBlock);
// Fill in H.
*H.block(0, 0, true) = sm::eigen::randomCovariance<2>() * 100;
*H.block(1, 1, true) = sm::eigen::randomCovariance<2>() * 100;
*H.block(2, 2, true) = sm::eigen::randomCovariance<3>() * 100;
*H.block(3, 3, true) = sm::eigen::randomCovariance<3>() * 100;
*H.block(4, 4, true) = sm::eigen::randomCovariance<3>() * 100;
// Start with two off diagonals.
H.block(0, 4, true)->setRandom();
H.block(0, 4, true)->array() *= 100;
H.block(1, 4, true)->setRandom();
H.block(1, 4, true)->array() *= 100;
//std::cout << "H:\n" << H << std::endl;
applySchurComplement(H,
e,
lambda,
marginalizedStartingBlock,
true,
*A,
invVi,
b);
Eigen::MatrixXd Hd = H.toDense();
Eigen::MatrixXd U = Hd.topLeftCorner(marginalizedStartingIndex, marginalizedStartingIndex);
Eigen::MatrixXd V = Hd.bottomRightCorner(H.rows() - marginalizedStartingIndex, H.rows() - marginalizedStartingIndex);
Eigen::MatrixXd W = Hd.topRightCorner(marginalizedStartingIndex, H.rows() - marginalizedStartingIndex);
V.diagonal().array() += lambda;
Eigen::MatrixXd AA = U - W * V.inverse() * W.transpose();
AA.diagonal().array() += lambda;
Eigen::VectorXd epsSparse = e.tail(e.size() - marginalizedStartingIndex);
Eigen::VectorXd epsDense = e.head(marginalizedStartingIndex);
Eigen::VectorXd bb = epsDense - W * V.inverse() * epsSparse;
{
SCOPED_TRACE("");
Eigen::MatrixXd Asa = A->toDense().selfadjointView<Eigen::Upper>();
sm::eigen::assertNear(Asa, AA, 1e-12, SM_SOURCE_FILE_POS, "Testing the lhs schur complement");
}
{
SCOPED_TRACE("");
sm::eigen::assertNear(b, bb, 1e-12, SM_SOURCE_FILE_POS, "Testing the rhs schur complement");
}
// Let's try it again to make sure stuff gets initialized correctly.
applySchurComplement(H,
e,
lambda,
marginalizedStartingBlock,
true,
*A,
invVi,
b);
{
SCOPED_TRACE("");
Eigen::MatrixXd Asa = A->toDense().selfadjointView<Eigen::Upper>();
sm::eigen::assertNear(Asa, AA, 1e-12, SM_SOURCE_FILE_POS, "Testing the lhs schur complement");
}
{
SCOPED_TRACE("");
sm::eigen::assertNear(b, bb, 1e-12, SM_SOURCE_FILE_POS, "Testing the rhs schur complement");
}
// Now we check the update function.
Eigen::VectorXd dx(marginalizedStartingIndex);
dx.setRandom();
Eigen::VectorXd denseDs = V.inverse() * (epsSparse - W.transpose() * dx);
for (int i = 0; i < H.bRows() - marginalizedStartingBlock; ++i) {
Eigen::VectorXd outDsi;
buildDsi(i, H, e, marginalizedStartingBlock, invVi[i], dx, outDsi);
Eigen::VectorXd dsi = denseDs.segment(H.rowBaseOfBlock(i + marginalizedStartingBlock) - marginalizedStartingIndex, H.rowsOfBlock(i + marginalizedStartingBlock));
sm::eigen::assertNear(outDsi, dsi, 1e-12, SM_SOURCE_FILE_POS, "Checking the update step calculation");
}
} catch (const std::exception& e) {
FAIL() << "Exception: " << e.what();
}
}
void CDKF::measurementUpdate(const ros::Time& prev_stamp,
const ros::Time& current_stamp,
const double image_angular_velocity,
const IMUList& imu_rotations,
const bool calc_offset) {
// convert tracked points to measurement
Eigen::VectorXd real_measurement(kMeasurementSize);
accessM(real_measurement, MEASURED_TIMESTAMP).array() =
(current_stamp - zero_timestamp_).toSec();
accessM(real_measurement, ANGULAR_VELOCITY).array() = image_angular_velocity;
if (verbose_) {
ROS_INFO_STREAM("Measured Values: \n" << real_measurement.transpose());
}
// create sigma points
MeasurementSigmaPoints sigma_points(
state_, cov_, measurement_noise_sd_, CDKF::stateToMeasurementEstimate,
imu_rotations, zero_timestamp_, calc_offset);
// get mean and cov
Eigen::VectorXd predicted_measurement;
sigma_points.calcEstimatedMean(&predicted_measurement);
Eigen::MatrixXd innovation;
sigma_points.calcEstimatedCov(&innovation);
if (verbose_) {
ROS_INFO_STREAM("Predicted Measurements: \n"
<< predicted_measurement.transpose());
}
// calc mah distance
Eigen::VectorXd diff = real_measurement - predicted_measurement;
double mah_dist = std::sqrt(diff.transpose() * innovation.inverse() * diff);
if (mah_dist > mah_threshold_) {
ROS_WARN("Outlier detected, measurement rejected");
return;
}
Eigen::MatrixXd cross_cov;
sigma_points.calcEstimatedCrossCov(&cross_cov);
Eigen::MatrixXd gain = cross_cov * innovation.inverse();
const Eigen::VectorXd state_diff =
gain * (real_measurement - predicted_measurement);
state_ += state_diff;
cov_ -= gain * innovation * gain.transpose();
if (verbose_) {
ROS_INFO_STREAM("Updated State: \n" << state_.transpose());
ROS_INFO_STREAM("Updated Cov: \n" << cov_);
}
// guard against precision issues
constexpr double kMaxTime = 10000.0;
if (accessS(state_, STATE_TIMESTAMP)[0] > kMaxTime) {
rezeroTimestamps(current_stamp);
}
}
