int softmax<T>::predict(cv::Mat const &input)
{
Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
input.rows,
input.step / sizeof(T));
return predict(Map.block(0, 0, input.rows, input.cols));
}
//==============================================================================
void NullFunction::evalHessian(
const Eigen::VectorXd& _x,
Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess)
{
_Hess.resize(pow(_x.size(),2));
_Hess.setZero();
}
/// Compute the Root Mean Square Error of the residuals
static double RMSE(const SfM_Data & sfm_data)
{
// Compute residuals for each observation
std::vector<double> vec;
for(Landmarks::const_iterator iterTracks = sfm_data.GetLandmarks().begin();
iterTracks != sfm_data.GetLandmarks().end();
++iterTracks)
{
const Observations & obs = iterTracks->second.obs;
for(Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
const View * view = sfm_data.GetViews().find(itObs->first)->second.get();
const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
const std::shared_ptr<cameras::IntrinsicBase> intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic);
const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
//std::cout << residual << " ";
vec.push_back( residual(0) );
vec.push_back( residual(1) );
}
}
const Eigen::Map<Eigen::RowVectorXd> residuals(&vec[0], vec.size());
const double RMSE = std::sqrt(residuals.squaredNorm() / vec.size());
return RMSE;
}
std::vector<int> const& softmax<T>::
batch_predicts(cv::Mat const &input)
{
Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
input.rows,
input.step / sizeof(T));
return batch_predicts(Map.block(0, 0, input.rows, input.cols));
}
void ExpMapQuaternion::applyDiffPseudoLog0_(
RefMat out, const ConstRefMat& in, const ConstRefVec& x, ReusableTemporaryMap& m)
{
mnf_assert(in.cols() == InputDim_ && "Dimensions mismatch" );
Eigen::Map<Eigen::MatrixXd, Eigen::Aligned> a = m.getMap(in.rows(),OutputDim_);
a.noalias() = in*diffPseudoLog0_(x);
out = a;
}
void evalGradient(const Eigen::VectorXd& _x,
Eigen::Map<Eigen::VectorXd> _grad) override
{
computeResultVector(_x);
_grad.setZero();
int smaller = std::min(mResultVector.size(), _grad.size());
for(int i=0; i < smaller; ++i)
_grad[i] = mResultVector[i];
}
void RigidBody3DState::updateMandMinv()
{
assert( unsigned( m_M0.nonZeros() ) == 6 * nbodies() );
assert( m_M0.nonZeros() == m_Minv0.nonZeros() );
assert( unsigned( m_M.nonZeros() ) == 12 * nbodies() );
assert( m_M.nonZeros() == m_Minv.nonZeros() );
for( unsigned bdy_idx = 0; bdy_idx < m_nbodies; ++bdy_idx )
{
// Orientation of the ith body
const Eigen::Map<const Matrix33sr> R{ m_q.segment<9>( 3 * m_nbodies + 9 * bdy_idx ).data() };
assert( fabs( ( R * R.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() ) <= 1.0e-9 );
assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
// Inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> I{ &m_M.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> I0{ &m_M0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
I = R * I0.asDiagonal() * R.transpose();
assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( I.determinant() > 0.0 );
}
// Inverse of the inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> Iinv{ &m_Minv.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> Iinv0{ &m_Minv0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
Iinv = R * Iinv0.asDiagonal() * R.transpose();
assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( Iinv.determinant() > 0.0 );
}
}
assert( MathUtilities::isIdentity( m_M * m_Minv, 1.0e-9 ) );
}
//Version 4
double basicBayes::evaluteMVG(std::vector<double>& sampleVecVect, std::vector<double>& meanVecVect, std::vector<double>& covMatVect){
//this funciton is problem specific
//Convert
Eigen::Map<Eigen::MatrixXd> sampleVec = convertVect(sampleVecVect);
Eigen::Map<Eigen::MatrixXd> meanVec = convertVect(meanVecVect);
Eigen::Map<Eigen::MatrixXd> covMat = convertCovMat(covMatVect);
Eigen::MatrixXd error = (sampleVec - meanVec);
Eigen::Matrix<double,1,1> secondHalf = (error.transpose()*covMat.inverse()*error);
return (1.0/(pow(2.0*M_PI,meanVec.rows()/2.0)*pow(covMat.determinant(),0.5)))*exp(-0.5*secondHalf(0,0));
}
void ComputePcBoundaries(const pcl::PointCloud<pcl::PointXYZ>&
pc, Eigen::Vector3f& min, Eigen::Vector3f& max) {
const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > x = pc.getMatrixXfMap(1, 4, 0); // this works for PointXYZRGBNormal
const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > y = pc.getMatrixXfMap(1, 4, 1); // this works for PointXYZRGBNormal
const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > z = pc.getMatrixXfMap(1, 4, 2); // this works for PointXYZRGBNormal
min << x.minCoeff(), y.minCoeff(), z.minCoeff();
max << x.maxCoeff(), y.maxCoeff(), z.maxCoeff();
}
std::vector<VectorEntry<Kernel>> mapParameter(Eigen::Map<Derived>& map) {
new(&map) Eigen::Map<Derived>(&m_parameters(++m_parameterOffset));
std::vector<VectorEntry<Kernel>> result(map.rows());
for (int i = 0; i < map.rows(); ++i)
result[i] = {m_parameterOffset + i, &m_parameters(m_parameterOffset + i)};
//minus one as we increase the parameter offset already in this function
m_parameterOffset+= (map.rows()-1);
return result;
};
static SparseMatrixsc formWorldSpaceInverseMassMatrix( const std::vector<scalar>& M, const std::vector<Vector3s>& I0, const std::vector<VectorXs>& R )
{
assert( M.size() == I0.size() );
assert( M.size() == I0.size() );
const unsigned nbodies{ static_cast<unsigned>( I0.size() ) };
const unsigned nvdofs{ 6 * nbodies };
SparseMatrixsc Mbody{ static_cast<SparseMatrixsc::Index>( nvdofs ), static_cast<SparseMatrixsc::Index>( nvdofs ) };
{
VectorXi column_nonzeros{ nvdofs };
column_nonzeros.segment( 0, 3 * nbodies ).setOnes();
column_nonzeros.segment( 3 * nbodies, 3 * nbodies ).setConstant( 3 );
Mbody.reserve( column_nonzeros );
}
// Load the total masses
for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
{
for( unsigned dof_idx = 0; dof_idx < 3; ++dof_idx )
{
const unsigned col{ 3 * bdy_idx + dof_idx };
const unsigned row{ col };
assert( M[ bdy_idx ] > 0.0 );
Mbody.insert( row, col ) = 1.0 / M[ bdy_idx ];
}
}
// Load the inertia tensors
for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
{
// Transform from principal axes rep
const Eigen::Map<const Matrix33sr> Rmat{ R[bdy_idx].data() };
assert( ( Rmat * Rmat.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
assert( fabs( Rmat.determinant() - 1.0 ) <= 1.0e-9 );
const Matrix33sr Iinv = Rmat * I0[bdy_idx].array().inverse().matrix().asDiagonal() * Rmat.transpose();
assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( Iinv.determinant() > 0.0 );
for( unsigned row_idx = 0; row_idx < 3; ++row_idx )
{
const unsigned col{ 3 * nbodies + 3 * bdy_idx + row_idx };
for( unsigned col_idx = 0; col_idx < 3; ++col_idx )
{
const unsigned row{ 3 * nbodies + 3 * bdy_idx + col_idx };
Mbody.insert( row, col ) = Iinv( row_idx, col_idx );
}
}
}
assert( 12 * nbodies == unsigned( Mbody.nonZeros() ) );
Mbody.makeCompressed();
return Mbody;
}
double BSplineMotionError<SPLINE_T>::evaluateErrorImplementation()
{
// the error is a scalar: c' Q c, with c the vector valued spline coefficients stacked
const double* cMat = &((_splineDV->spline()).coefficients()(0,0));
Eigen::Map<const Eigen::VectorXd> c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength);
// Q*c :
// create result container:
Eigen::VectorXd Qc(_Q.rows()); // number of rows of Q:
Qc.setZero();
// std::cout << Qc->rows() << ":" << Qc->cols() << std::endl;
_Q.multiply(&Qc, c);
return c.transpose() * (Qc);
}
void integrate_inplace(Eigen::Map<MatrixType> integral, Callable&& f) const noexcept
{
for (std::size_t index{0}; index < points(); ++index)
{
integral.noalias() += f(femvals[index], index) * m_weights[index];
}
}
// [[Rcpp::export]]
Eigen::MatrixXd cholupdateL_rcpp(const Eigen::Map<Eigen::MatrixXd>& L,
const Eigen::Map<Eigen::MatrixXd>& V12,
const Eigen::Map<Eigen::MatrixXd>& V22) {
int k = L.rows();
int k2 = V22.rows();
Eigen::MatrixXd S(k, k2);
Eigen::MatrixXd U(k2, k2);
Eigen::MatrixXd M(k2, k2);
Eigen::MatrixXd Lup(k+k2, k+k2);
Lup.setZero();
S = L.triangularView<Lower>().solve(V12);
M = V22 - S.adjoint() * S ;
U = M.adjoint().llt().matrixL();
Lup.topLeftCorner(k,k) = L;
Lup.bottomLeftCorner(k2,k) = S.adjoint();
Lup.bottomRightCorner(k2,k2) = U;
return Lup;
}
double BodyNode::VelocityObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());
// Update forward kinematics information with _x
// We are just insterested in spacial velocity of mBodyNode
mBodyNode->getParentJoint()->setGenVels(_x, true, false);
// Compute and return the geometric distance between body node transformation
// and target transformation
Eigen::Vector6d diff = mBodyNode->getWorldVelocity() - mVelocity;
return diff.dot(diff);
}
Rcpp::List GetIndCEScoresCPP( const Eigen::Map<Eigen::VectorXd> & yVec, const Eigen::Map<Eigen::VectorXd> & muVec, const Eigen::Map<Eigen::VectorXd> & lamVec, const Eigen::Map<Eigen::MatrixXd> & phiMat, const Eigen::Map<Eigen::MatrixXd> & SigmaYi){
// Setting up initial values
const unsigned int lenlamVec = lamVec.size();
Eigen::MatrixXd xiVar = Eigen::MatrixXd::Constant(lenlamVec,lenlamVec,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd xiEst = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd fittedY = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
Eigen::MatrixXd LamPhi = lamVec.asDiagonal() * phiMat.transpose();
Eigen::LDLT<Eigen::MatrixXd> ldlt_SigmaYi(SigmaYi);
xiEst = LamPhi * ldlt_SigmaYi.solve(yVec - muVec) ; // LamPhiSig * (yVec - muVec);
xiVar = -LamPhi * ldlt_SigmaYi.solve(LamPhi.transpose()); // LamPhiSig.transpose();
xiVar.diagonal() += lamVec;
fittedY = muVec + phiMat * xiEst;
return Rcpp::List::create(Rcpp::Named("xiEst") = xiEst,
Rcpp::Named("xiVar") = xiVar,
Rcpp::Named("fittedY") = fittedY);
}
double BodyNode::TransformObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());
// Update forward kinematics information with _x
// We are just insterested in transformation of mBodyNode
mBodyNode->getParentJoint()->setConfigs(_x, true, false, false);
// Compute and return the geometric distance between body node transformation
// and target transformation
Eigen::Isometry3d bodyT = mBodyNode->getWorldTransform();
Eigen::Vector6d dist = math::logMap(bodyT.inverse() * mT);
return dist.dot(dist);
}
void DiagonalGMM::sampleWithLimits( Eigen::Map<Eigen::VectorXf> &dst, const Eigen::VectorXf &minValues, const Eigen::VectorXf &maxValues )
{
//printf("sample with limits \n");
int idx=sampleComponent();
//printf("sampleWithLimits selected component %d\n",idx);
if (dst.rows()!=nDimensions)
Debug::throwError("Invalid dst dimensions for sampleWithLimits()!");
for (int d=0; d<nDimensions; d++)
{
//dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
//printf(" Sample with limit for %d-- mean %f, stdv %f, min %f, max %f \n",d,mean[idx][d],std[idx][d],minValues[d],maxValues[d] );
dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
}
}
/* =================================================================
GIBBS SAMPLER ALGORITHM TO DRAW A NEW VECTOR "y" ~ TruncNormal of dimension D
GIVEN PREVIOUS VECTOR "y" AS INPUT
Visits each entry in y in random order,
and resamples that entry using efficient 1D truncated normal sampling routines.
When we visit the *target* dimension,
we enforce that it must be larger than the maximum of all other entries
Otherwise, we visit other dimensions and enforce that they must be smaller than y[target]
*/
void gibbs_sample_mv_randn_trunc( VectorType& y, const Eigen::Map<VectorType>& mu, int targetDim) {
int D = (int) mu.size();
double maxOthers;
int *perm = new int[D];
randperm( D, perm );
for (int dd=0; dd < D; dd++) {
int d = perm[dd];
if (d == targetDim) {
maxOthers = get_max_value_ignore_dim( y, targetDim );
y(d) = randn_trunc_below( mu(d), 1.0, maxOthers );
} else {
y(d) = randn_trunc_above( mu(d), 1.0, y(targetDim) );
}
}
delete [] perm;
perm = NULL;
}
// Correlation implementation in Eigen
//' @rdname corFamily
//' @export
// [[Rcpp::export]]
Eigen::MatrixXd corEigen(Eigen::Map<Eigen::MatrixXd> & X) {
// Handle degenerate cases
if (X.rows() == 0 && X.cols() > 0) {
return Eigen::MatrixXd::Constant(X.cols(), X.cols(),
Rcpp::NumericVector::get_na());
}
// Computing degrees of freedom
// n - 1 is the unbiased estimate whereas n is the MLE
const int df = X.rows() - 1; // Subtract 1 by default
X.rowwise() -= X.colwise().mean(); // Centering
Eigen::MatrixXd cor = X.transpose() * X / df; // The covariance matrix
// Get 1 over the standard deviations
Eigen::VectorXd inv_sds = cor.diagonal().array().sqrt().inverse();
// Scale the covariance matrix
cor = cor.cwiseProduct(inv_sds * inv_sds.transpose());
return cor;
}
//==============================================================================
void NullFunction::evalGradient(const Eigen::VectorXd& _x,
Eigen::Map<Eigen::VectorXd> _grad)
{
_grad.resize(_x.size());
_grad.setZero();
}
//' Fast Matrix Inverse
//'
//' Computes using RcppEigen the inverse of A
//'
//' @param A is the matrix being inverted
//'
//' @return inverse of A
//'
//' @examples
//' \dontrun{
//' rcppeigen_fsolve(A)
//' }
//'
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_fsolve(const Eigen::Map<Eigen::MatrixXd> & A){
Eigen::MatrixXd Ainv = A.inverse();
return Ainv;
}
//' Fast Matrix Determinant
//'
//' Computes using RcppEigen the determinant of A
//'
//' @param A is the matrix whose determinant calculated
//'
//' @return determinant of A
//'
//' @examples
//' \dontrun{
//' rcppeigen_fdet(A)
//' }
//'
//[[Rcpp::export]]
double rcppeigen_fdet(const Eigen::Map<Eigen::MatrixXd> & A){
return A.determinant();
}
//' Fast Matrix T-Cross-Product
//'
//' Computes using RcppEigen the product of A and t(B)
//'
//' @param A is the first parameter in A times t(B)
//' @param B is the second parameter in A times t(B)
//'
//' @return matrix tcross-product A times t(B)
//'
//' @examples
//' \dontrun{
//' rcppeigen_ftcrossprod(A, B)
//' }
//'
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_ftcrossprod(const Eigen::Map<Eigen::MatrixXd> & A,
const Eigen::Map<Eigen::MatrixXd> & B){
return A * B.transpose();
}
//' Fast Matrix Cross-Product
//'
//' Computes using RcppEigen the product of t(A) and B
//'
//' @param A is the first parameter in t(A) times B
//' @param B is the second parameter in t(A) times B
//'
//' @return matrix cross-product t(A) times B
//'
//' @examples
//' \dontrun{
//' rcppeigen_fcrossprod(A, B)
//' }
//'
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_fcrossprod(const Eigen::Map<Eigen::MatrixXd> & A,
const Eigen::Map<Eigen::MatrixXd> & B){
return A.transpose() * B;
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
// The RF is formed as this x_axis | y_axis | normal
Eigen::Map<Eigen::Vector3f> x_axis (rf);
Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
Eigen::Map<Eigen::Vector3f> normal (rf + 6);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
if (neighb_cnt == 0)
{
for (size_t i = 0; i < desc.size (); ++i)
desc[i] = std::numeric_limits<float>::quiet_NaN ();
memset (rf, 0, sizeof (rf[0]) * 9);
return (false);
}
float minDist = std::numeric_limits<float>::max ();
int minIndex = -1;
for (size_t i = 0; i < nn_indices.size (); i++)
{
if (nn_dists[i] < minDist)
{
minDist = nn_dists[i];
minIndex = nn_indices[i];
}
}
// Get origin point
Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
// Get origin normal
// Use pre-computed normals
normal = normals[minIndex].getNormalVector3fMap ();
// Compute and store the RF direction
x_axis[0] = rnd ();
x_axis[1] = rnd ();
x_axis[2] = rnd ();
if (!pcl::utils::equal (normal[2], 0.0f))
x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
else if (!pcl::utils::equal (normal[1], 0.0f))
x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
else if (!pcl::utils::equal (normal[0], 0.0f))
x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
x_axis.normalize ();
// Check if the computed x axis is orthogonal to the normal
assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
// Store the 3rd frame vector
y_axis = normal.cross (x_axis);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal (nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
/// ----- Compute current neighbour polar coordinates -----
/// Get distance between the neighbour and the origin
float r = sqrt (nn_dists[ne]);
/// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
/// Normalize to compute the dot product
proj.normalize ();
/// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));
// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_+1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad-1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_+1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang-1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang-1;
break;
}
}
// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_distances;
int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
// point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
if (point_density == 0)
continue;
float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
(k*radius_bins_) +
j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
memset (rf, 0, sizeof (rf[0]) * 9);
return (true);
}
Eigen::MatrixXd RmullwlskCC( const Eigen::Map<Eigen::VectorXd> & bw, const std::string kernel_type, const Eigen::Map<Eigen::MatrixXd> & tPairs, const Eigen::Map<Eigen::MatrixXd> & cxxn, const Eigen::Map<Eigen::VectorXd> & win, const Eigen::Map<Eigen::VectorXd> & xgrid, const Eigen::Map<Eigen::VectorXd> & ygrid, const bool & bwCheck){
// tPairs : xin (in MATLAB code)
// cxxn : yin (in MATLAB code)
// xgrid: out1 (in MATLAB code)
// ygrid: out2 (in MATLAB code)
// bwCheck : boolean/ cause the function to simply run the bandwidth check.
const double invSqrt2pi= 1./(sqrt(2.*M_PI));
// Map the kernel name so we can use switches
std::map<std::string,int> possibleKernels;
possibleKernels["epan"] = 1; possibleKernels["rect"] = 2;
possibleKernels["gauss"] = 3; possibleKernels["gausvar"] = 4;
possibleKernels["quar"] = 5;
// The following test is here for completeness, we mightwant to move it up a
// level (in the wrapper) in the future.
// If the kernel_type key exists set KernelName appropriately
int KernelName = 0;
if ( possibleKernels.count( kernel_type ) != 0){
KernelName = possibleKernels.find( kernel_type )->second; //Set kernel choice
} else {
// otherwise use "epan"as the kernel_type
//Rcpp::Rcout << "Kernel_type argument was not set correctly; Epanechnikov kernel used." << std::endl;
Rcpp::warning("Kernel_type argument was not set correctly; Epanechnikov kernel used.");
KernelName = possibleKernels.find( "epan" )->second;;
}
// Check that we do not have zero weights // Should do a try-catch here
// Again this might be best moved a level-up.
if ( !(win.all()) ){ //
Rcpp::Rcout << "Cases with zero-valued windows are not yet implemented" << std::endl;
return (tPairs);
}
// Start the actual smoother here
unsigned int xgridN = xgrid.size();
unsigned int ygridN = ygrid.size();
Eigen::MatrixXd mu(xgrid.size(), ygrid.size());
mu.setZero();
const double bufSmall = 1e-6; // pow(double(10),-6);
for (unsigned int j = 0; j != ygridN; ++j) {
for (unsigned int i = 0; i != xgridN; ++i) {
//locating local window (LOL) (bad joke)
std::vector <unsigned int> indx;
//if the kernel is not Gaussian
if ( KernelName != 3) {
//construct listX as vectors / size is unknown originally
for (unsigned int y = 0; y != tPairs.cols(); y++){
if ( (std::abs( tPairs(0,y) - xgrid(i) ) <= (bw(0)+ bufSmall ) && std::abs( tPairs(1,y) - ygrid(j) ) <= (bw(1)+ bufSmall)) ) {
indx.push_back(y);
}
}
} else{ // just get the whole deal
for (unsigned int y = 0; y != tPairs.cols(); ++y){
indx.push_back(y);
}
}
// for (unsigned int y = 0; y != indx.size(); ++y){
// Rcpp::Rcout << "indx.at(y): " << indx.at(y)<< ", ";
// }
unsigned int indxSize = indx.size();
Eigen::VectorXd lw(indxSize);
Eigen::VectorXd ly(indxSize);
Eigen::MatrixXd lx(2,indxSize);
for (unsigned int u = 0; u !=indxSize; ++u){
lx.col(u) = tPairs.col(indx[u]);
lw(u) = win(indx[u]);
ly(u) = cxxn(indx[u]);
}
// check enough points are in the local window
unsigned int meter=1;
for (unsigned int u =0; u< indxSize; ++u) {
for (unsigned int t = u + 1; t < indxSize; ++t) {
if ( (lx(0,u) != lx(0,t) ) || (lx(1,u) != lx(1,t) ) ) {
meter++;
}
}
if (meter >= 3) {
break;
}
}
//computing weight matrix
if (meter >= 3 && !bwCheck) {
Eigen::VectorXd temp(indxSize);
Eigen::MatrixXd llx(2, indxSize );
llx.row(0) = (lx.row(0).array() - xgrid(i))/bw(0);
llx.row(1) = (lx.row(1).array() - ygrid(j))/bw(1);
//define the kernel used
switch (KernelName){
case 1: // Epan
temp= ((1-llx.row(0).array().pow(2))*(1- llx.row(1).array().pow(2))).array() *
((9./16)*lw).transpose().array();
break;
case 2 : // Rect
temp=(lw.array())*.25 ;
break;
case 3 : // Gauss
temp = ((-.5*(llx.row(1).array().pow(2))).exp()) * invSqrt2pi *
((-.5*(llx.row(0).array().pow(2))).exp()) * invSqrt2pi *
(lw.transpose().array());
break;
case 4 : // GausVar
temp = (lw.transpose().array()) *
((-0.5 * llx.row(0).array().pow(2)).array().exp() * invSqrt2pi).array() *
((-0.5 * llx.row(1).array().pow(2)).array().exp() * invSqrt2pi).array() *
(1.25 - (0.25 * (llx.row(0).array().pow(2))).array()) *
(1.50 - (0.50 * (llx.row(1).array().pow(2))).array());
break;
case 5 : // Quar
temp = (lw.transpose().array()) *
((1.-llx.row(0).array().pow(2)).array().pow(2)).array() *
((1.-llx.row(1).array().pow(2)).array().pow(2)).array() * (225./256.);
break;
}
// make the design matrix
Eigen::MatrixXd X(indxSize ,3);
X.setOnes();
X.col(1) = lx.row(0).array() - xgrid(i);
X.col(2) = lx.row(1).array() - ygrid(j);
Eigen::LDLT<Eigen::MatrixXd> ldlt_XTWX(X.transpose() * temp.asDiagonal() *X);
// The solver should stop if the value is NaN. See the HOLE example in gcvlwls2dV2.
// Rcpp::Rcout << X << std::endl;
Eigen::VectorXd beta = ldlt_XTWX.solve(X.transpose() * temp.asDiagonal() * ly);
mu(i,j)=beta(0);
} else if(meter < 3){
// Rcpp::Rcout <<"The meter value is:" << meter << std::endl;
if (bwCheck) {
Eigen::MatrixXd checker(1,1);
checker(0,0) = 0.;
return(checker);
} else {
Rcpp::stop("No enough points in local window, please increase bandwidth.");
}
}
}
}
if (bwCheck){
Eigen::MatrixXd checker(1,1);
checker(0,0) = 1.;
return(checker);
}
return ( mu );
}
Eigen::Matrix<double, 4, 3> ExpMapQuaternion::diffRetractation_(const ConstRefVec& x)
{
const Eigen::Map<const Eigen::Quaterniond> xQ(x.data());
Eigen::Matrix<double, 4, 3> J;
J << 0.5*xQ.w(), -0.5*xQ.z(), 0.5*xQ.y(),
0.5*xQ.z(), 0.5*xQ.w(), -0.5*xQ.x(),
-0.5*xQ.y(), 0.5*xQ.x(), 0.5*xQ.w(),
-0.5*xQ.x(), -0.5*xQ.y(), -0.5*xQ.z();
return J;
}
inline bool Generate_SfM_Report
(
const SfM_Data & sfm_data,
const std::string & htmlFilename
)
{
// Compute mean,max,median residual values per View
IndexT residualCount = 0;
Hash_Map< IndexT, std::vector<double> > residuals_per_view;
for ( const auto & iterTracks : sfm_data.GetLandmarks() )
{
const Observations & obs = iterTracks.second.obs;
for ( const auto & itObs : obs )
{
const View * view = sfm_data.GetViews().at(itObs.first).get();
const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
const cameras::IntrinsicBase * intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
// Use absolute values
const Vec2 residual = intrinsic->residual(pose, iterTracks.second.X, itObs.second.x).array().abs();
residuals_per_view[itObs.first].push_back(residual(0));
residuals_per_view[itObs.first].push_back(residual(1));
++residualCount;
}
}
using namespace htmlDocument;
// extract directory from htmlFilename
const std::string sTableBegin = "<table border=\"1\">",
sTableEnd = "</table>",
sRowBegin= "<tr>", sRowEnd = "</tr>",
sColBegin = "<td>", sColEnd = "</td>",
sNewLine = "<br>", sFullLine = "<hr>";
htmlDocument::htmlDocumentStream htmlDocStream("SFM report.");
htmlDocStream.pushInfo(
htmlDocument::htmlMarkup("h1", std::string("SFM report.")));
htmlDocStream.pushInfo(sFullLine);
htmlDocStream.pushInfo( "Dataset info:" + sNewLine );
std::ostringstream os;
os << " #views: " << sfm_data.GetViews().size() << sNewLine
<< " #poses: " << sfm_data.GetPoses().size() << sNewLine
<< " #intrinsics: " << sfm_data.GetIntrinsics().size() << sNewLine
<< " #tracks: " << sfm_data.GetLandmarks().size() << sNewLine
<< " #residuals: " << residualCount << sNewLine;
htmlDocStream.pushInfo( os.str() );
htmlDocStream.pushInfo( sFullLine );
htmlDocStream.pushInfo( sTableBegin);
os.str("");
os << sRowBegin
<< sColBegin + "IdView" + sColEnd
<< sColBegin + "Basename" + sColEnd
<< sColBegin + "#Observations" + sColEnd
<< sColBegin + "Residuals min" + sColEnd
<< sColBegin + "Residuals median" + sColEnd
<< sColBegin + "Residuals mean" + sColEnd
<< sColBegin + "Residuals max" + sColEnd
<< sRowEnd;
htmlDocStream.pushInfo( os.str() );
for (const auto & iterV : sfm_data.GetViews() )
{
const View * v = iterV.second.get();
const IndexT id_view = v->id_view;
os.str("");
os << sRowBegin
<< sColBegin << id_view << sColEnd
<< sColBegin + stlplus::basename_part(v->s_Img_path) + sColEnd;
// IdView | basename | #Observations | residuals min | residual median | residual max
if (sfm_data.IsPoseAndIntrinsicDefined(v))
{
if( residuals_per_view.find(id_view) != residuals_per_view.end() )
{
const std::vector<double> & residuals = residuals_per_view.at(id_view);
if (!residuals.empty())
{
double min, max, mean, median;
minMaxMeanMedian(residuals.begin(), residuals.end(), min, max, mean, median);
os << sColBegin << residuals.size()/2 << sColEnd // #observations
<< sColBegin << min << sColEnd
<< sColBegin << median << sColEnd
<< sColBegin << mean << sColEnd
<< sColBegin << max <<sColEnd;
}
}
}
os << sRowEnd;
htmlDocStream.pushInfo( os.str() );
}
htmlDocStream.pushInfo( sTableEnd );
htmlDocStream.pushInfo( sFullLine );
// combine all residual values into one vector
// export the SVG histogram
{
IndexT residualCount = 0;
for (Hash_Map< IndexT, std::vector<double> >::const_iterator
it = residuals_per_view.begin();
it != residuals_per_view.end();
++it)
{
residualCount += it->second.size();
}
// Concat per view residual values into one vector
std::vector<double> residuals(residualCount);
residualCount = 0;
for (Hash_Map< IndexT, std::vector<double> >::const_iterator
it = residuals_per_view.begin();
it != residuals_per_view.end();
++it)
{
std::copy(it->second.begin(),
it->second.begin()+it->second.size(),
residuals.begin()+residualCount);
residualCount += it->second.size();
}
if (!residuals.empty())
{
// RMSE computation
const Eigen::Map<Eigen::RowVectorXd> residuals_mapping(&residuals[0], residuals.size());
const double RMSE = std::sqrt(residuals_mapping.squaredNorm() / (double)residuals.size());
os.str("");
os << sFullLine << "SfM Scene RMSE: " << RMSE << sFullLine;
htmlDocStream.pushInfo(os.str());
const double maxRange = *max_element(residuals.begin(), residuals.end());
Histogram<double> histo(0.0, maxRange, 100);
histo.Add(residuals.begin(), residuals.end());
svg::svgHisto svg_Histo;
svg_Histo.draw(histo.GetHist(), std::pair<float,float>(0.f, maxRange),
stlplus::create_filespec(stlplus::folder_part(htmlFilename), "residuals_histogram", "svg"),
600, 200);
os.str("");
os << sNewLine<< "Residuals histogram" << sNewLine;
os << "<img src=\""
<< "residuals_histogram.svg"
<< "\" height=\"300\" width =\"800\">\n";
htmlDocStream.pushInfo(os.str());
}
}
std::ofstream htmlFileStream(htmlFilename.c_str());
htmlFileStream << htmlDocStream.getDoc();
const bool bOk = !htmlFileStream.bad();
return bOk;
}
//[[Rcpp::export]]
List SPMBgraphsqrt(Eigen::Map<Eigen::MatrixXd> data, NumericVector &lambda, int nlambda, int d, NumericVector &x, IntegerVector &col_cnz, IntegerVector &row_idx)
{
Eigen::ArrayXd Xb, r, grad, w1, Y, XX, gr;
Eigen::ArrayXXd X;
Eigen::MatrixXd tmp_icov;
tmp_icov.resize(d, d);
tmp_icov.setZero();
std::vector<Eigen::MatrixXd > tmp_icov_p;
tmp_icov_p.clear();
for(int i = 0; i < nlambda; i++)
tmp_icov_p.push_back(tmp_icov);
int n = data.rows();
X = data;
XX.resize(d);
for (int j = 0; j < d; j++)
XX[j] = (X.col(j)*X.col(j)).sum()/n;
double prec = 1e-4;
int max_iter = 1000;
int num_relaxation_round = 3;
int cnz = 0;
for(int m=0;m<d;m++)
{
Xb.resize(n);
Xb.setZero();
grad.resize(d);
grad.setZero();
gr.resize(d);
gr.setZero();
w1.resize(d);
w1.setZero();
r.resize(n);
r.setZero();
Y = X.col(m);
Eigen::ArrayXd Xb_master(n);
Eigen::ArrayXd w1_master(n);
std::vector<int> actset_indcat(d, 0);
std::vector<int> actset_indcat_master(d, 0);
std::vector<int> actset_idx;
std::vector<double> old_coef(d, 0);
std::vector<double> grad(d, 0);
std::vector<double> grad_master(d, 0);
double a = 0, g = 0, L = 0, sum_r = 0, sum_r2 = 0;
double tmp_change = 0, local_change = 0;
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
double dev_thr = fabs(L) * prec;
//cout<<dev_thr<<endl;
for(int i = 0; i < d; i++)
{
grad[i] = (r * X.col(i)).sum() / (n*L);
}
for(int i = 0; i < d; i++) gr[i] = abs(grad[i]);
w1_master = w1;
Xb_master = Xb;
for (int i = 0; i < d; i++) grad_master[i] = gr[i];
std::vector<double> stage_lambdas(d, 0);
for(int i=0;i<nlambda;i++)
{
double ilambda = lambda[i];
w1 = w1_master;
Xb = Xb_master;
for (int j = 0; j < d; j++)
{
gr[j] = grad_master[j];
actset_indcat[j] = actset_indcat_master[j];
}
// init the active set
double threshold;
if (i > 0)
threshold = 2 * lambda[i] - lambda[i - 1];
else
threshold = 2 * lambda[i];
for (int j = 0; j < m; ++j)
{
stage_lambdas[j] = lambda[i];
if (gr[j] > threshold) actset_indcat[j] = 1;
}
for (int j = m+1; j < d; ++j)
{
stage_lambdas[j] = lambda[i];
if (gr[j] > threshold) actset_indcat[j] = 1;
}
stage_lambdas[m] = lambda[i];
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
// loop level 0: multistage convex relaxation
int loopcnt_level_0 = 0;
int idx;
double old_w1, updated_coord;
while (loopcnt_level_0 < num_relaxation_round)
{
loopcnt_level_0++;
// loop level 1: active set update
int loopcnt_level_1 = 0;
bool terminate_loop_level_1 = true;
while (loopcnt_level_1 < max_iter)
{
loopcnt_level_1++;
terminate_loop_level_1 = true;
for (int j = 0; j < d; j++) old_coef[j] = w1[j];
// initialize actset_idx
actset_idx.clear();
for (int j = 0; j < m; j++)
if (actset_indcat[j])
{
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[j];
w1[j] = thresholdl1(g, stage_lambdas[j]) / a;
tmp = w1[j] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(j);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(j);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[j];
if (fabs(updated_coord) > 0) actset_idx.push_back(j);
}
for (int j = m+1; j < d; j++)
if (actset_indcat[j])
{
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[j];
w1[j] = thresholdl1(g, stage_lambdas[j]) / a;
tmp = w1[j] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(j);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(j);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[j];
if (fabs(updated_coord) > 0) actset_idx.push_back(j);
}
// loop level 2: proximal newton on active set
int loopcnt_level_2 = 0;
bool terminate_loop_level_2 = true;
while (loopcnt_level_2 < max_iter)
{
loopcnt_level_2++;
terminate_loop_level_2 = true;
for (int k = 0; k < actset_idx.size(); k++)
{
idx = actset_idx[k];
old_w1 = w1[idx];
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(idx) * X.col(idx);
g = (wXX * w1[idx] + r * X.col(idx)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[idx];
w1[idx] = thresholdl1(g, stage_lambdas[idx]) / a;
tmp = w1[idx] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(idx);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(idx);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[idx];
tmp_change = old_w1 - w1[idx];
double a = (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
local_change = a * tmp_change * tmp_change / (2 * L * n);
if (local_change > dev_thr)
terminate_loop_level_2 = false;
}
if (terminate_loop_level_2)
break;
}
terminate_loop_level_1 = true;
// check stopping criterion 1: fvalue change
for (int k = 0; k < actset_idx.size(); ++k)
{
idx = actset_idx[k];
tmp_change = old_w1 - w1[idx];
double a = (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
local_change = a * tmp_change * tmp_change / (2 * L * n);
if (local_change > dev_thr)
terminate_loop_level_1 = false;
}
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
if (terminate_loop_level_1)
break;
// check stopping criterion 2: active set change
bool new_active_idx = false;
for (int k = 0; k < m; k++)
if (actset_indcat[k] == 0)
{
grad[idx] = (r * X.col(idx)).sum() / (n*L);
//cout<<grad[idx];
gr[k] = fabs(grad[k]);
if (gr[k] > stage_lambdas[k])
{
actset_indcat[k] = 1;
new_active_idx = true;
}
}
for (int k = m+1; k < d; k++)
if (actset_indcat[k] == 0)
{
grad[idx] = (r * X.col(idx)).sum() / (n*L);
//cout<<grad[idx]
gr[k] = fabs(grad[k]);
if (gr[k] > stage_lambdas[k])
{
actset_indcat[k] = 1;
new_active_idx = true;
}
}
if(!new_active_idx)
break;
}
if (loopcnt_level_0 == 1)
{
for (int j = 0; j < d; j++)
{
w1_master[j] = w1[j];
grad_master[j] = gr[j];
actset_indcat_master[j] = actset_indcat[j];
}
for (int j = 0; j < n; j++) Xb_master[j] = Xb[j];
}
}
for(int j=0;j<actset_idx.size();j++)
{
int w_idx = actset_idx[j];
x[cnz] = w1[w_idx];
row_idx[cnz] = i*d+w_idx;
cnz++;
//cout<<cnz<<" ";
}
double tal = 0;
Eigen::MatrixXd temp;
temp.resize(n, 1);
for(int j = 0; j < n; j++)
{
temp(j, 0) = 0;
for(int k = 0; k < d; k++)
temp(j, 0) += X.matrix()(j, k)*w1[k];
temp(j, 0) = Y[j] - temp(j, 0);
}
//temp = Y.matrix() - X.matrix().transpose()*w1.matrix();
for(int j = 0; j < n; j++)
tal += temp(j, 0)*temp(j, 0);
tal = sqrt(tal)/sqrt(n);
tmp_icov = tmp_icov_p[i];
tmp_icov(m, m) = pow(tal, -2);
for(int j = 0; j < m; j++)
tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
for(int j = m+1; j < d; j++)
tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
tmp_icov_p[i] = tmp_icov;
}
col_cnz[m+1]=cnz;
}
for(int i = 0; i < nlambda; i++)
tmp_icov_p[i] = (tmp_icov_p[i].transpose()+tmp_icov_p[i])/2;
return List::create(
_["col_cnz"] = col_cnz,
_["row_idx"] = row_idx,
_["x"] = x,
_["icov"] = tmp_icov_p
);
}
