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 ); }