Triplets ThreeSum(vector<int> arr, int targetSum)
{
sort(arr.begin(), arr.end());
int length = arr.size();
Triplets ret;
for (int i = 0; i < length; i++)
{
//skip duplicate
if (i > 0 && arr[i] == arr[i - 1])
continue;
int left = i + 1;
int right = length - 1;
while (left < right)
{
int currSum = arr[i] + arr[left] + arr[right];
if (currSum > targetSum)
right--;
else if (currSum < targetSum)
left++;
else
{
vector<int> triplet(3);
triplet[0] = arr[i];
triplet[1] = arr[left];
triplet[2] = arr[right];
ret.push_back(triplet);
left++;
right--;
}
}
}
return ret;
}
unsigned n_nuc_differences(const Triplets& T,int i,int j)
{
unsigned n=0;
for(int pos=0;pos<3;pos++)
if (T.sub_nuc(i,pos) != T.sub_nuc(j,pos))
n++;
return n;
}
ublas::matrix<int> nucleotide_cost_matrix(const Triplets& T)
{
ublas::matrix<int> cost(T.size(), T.size());
for(int i=0;i<cost.size1();i++)
for(int j=0;j<cost.size2();j++)
cost(i,j) = n_nuc_differences(T,i,j);
return cost;
}
valarray<double> get_codon_frequencies_from_independent_nucleotide_frequencies(const Triplets& C,const valarray<double>& fN ) {
valarray<double> fC(C.size());
for(int i=0;i<fC.size();i++) {
fC[i] = 1.0;
for(int pos=0;pos<3;pos++)
fC[i] *= fN[ C.sub_nuc(i,pos) ];
}
fC /= fC.sum();
return fC;
}
valarray<double> get_nucleotide_counts_from_codon_counts(const Triplets& C,const valarray<double>& C_counts) {
const Nucleotides& N = C.getNucleotides();
valarray<double> N_counts(0.0,N.size());
// For each codon type
for(int i=0;i<C.size();i++) {
// For each position in the codon
for(int pos=0;pos<3;pos++)
// Cound the nucleotides that occur there
N_counts[ C.sub_nuc(i,pos) ] += C_counts[i];
}
return N_counts;
}
void PrintTriplets(const Triplets tri)
{
if (tri.size() == 0)
cout << "No data" << endl;
else
{
Triplets::const_iterator iter = tri.cbegin();
for (; iter != tri.cend(); iter++)
{
for (int i = 0; i < 3; i++)
cout << (*iter)[i] << "\t";
cout << endl;
}
}
}
void BuildProblem( Triplets& triplets, const Mat& rgb, const Mat& Known, const double epsilon=1e-5 )
{
typedef typename Triplets::value_type Trip;
const int nc(3), half_window_size(window_size/2), neb_size ( window_size * window_size );
const Mat known( erode( Known ) );
const Size size( rgb.size() );
const int lap_size ( size.area( ) );
const MatrixXi indsM ( Map<MatrixXi> ( VectorXi(VectorXi::LinSpaced( lap_size, 0, lap_size-1 )).data( ), size.height, size.width ) );
typedef double cType;
const Mat st(transpose(rgb).reshape(1));
MatrixXd Rgbm( st.cols, st.rows ); std::copy( st.begin<uchar>(), st.end<uchar>(), Rgbm.data() );
Rgbm /= 255;
vector<cType> sum(lap_size,0);
const Matrix< double, nc, nc > EpsEye ( (epsilon/neb_size) * Matrix< double, nc, nc >::Identity() );
for (int i( half_window_size ); i < size.height - half_window_size; ++i) {
for (int j( half_window_size ); j < size.width - half_window_size; ++j) {
if ( !known.at<uchar>(i, j) ){
const Matrix< double, window_size*nc, window_size> wb ( Rgbm.block<window_size*nc, window_size>( (i-1)*nc, j-1 ) );
const Matrix< double, nc, neb_size > Wk ( wb.data() );
const Matrix< double, nc, 1 > win_mu( Wk.rowwise( ).sum( )/neb_size );
const Matrix< double, nc, nc > mi ( Wk*Wk.transpose()/neb_size - win_mu*win_mu.transpose() + EpsEye ) ,Win_var ( mi.inverse( ) );
const Matrix< double, nc, neb_size > Wkm( Wk - win_mu.replicate<1,neb_size>() );
const Matrix <double, neb_size, neb_size> tvals( ( Matrix <double, neb_size, neb_size>::Ones() + Wkm.transpose( ) * Win_var * Wkm )/neb_size );
const Matrix <int, window_size, window_size> win_inds_block ( indsM.block<window_size, window_size>( i - half_window_size, j - half_window_size ) );
const Matrix <int, neb_size, 1> win_inds( win_inds_block.data( ) );
const Matrix <int, neb_size, neb_size> row_inds (win_inds. template replicate<1,neb_size>());
for (int r(0); r < tvals.rows( ); ++r) {
for(int c(0); c < tvals.cols( ); ++c ) {
const cType v( -tvals.coeff(r,c) );
if( abs(v) > epsilon ){
const int cr( row_inds(c, r) ), rc( row_inds(r, c) );
const Trip tp( cr, rc, v );
sum[ rc ] -= v;
triplets.push_back(tp);
}
}
}
}
}
}
for ( int i(0); i < lap_size; ++i ) {
triplets.push_back( Trip( i, i, sum[i] ) );
}
}
/// Filter inconsistent correspondences by using 3-view correspondences on view triplets
void SfM_Data_Structure_Estimation_From_Known_Poses::filter(
const SfM_Data & sfm_data,
const Pair_Set & pairs,
const std::shared_ptr<Regions_Provider> & regions_provider)
{
// Compute triplets
// Triangulate triplet tracks
// - keep valid one
typedef std::vector< graph::Triplet > Triplets;
const Triplets triplets = graph::tripletListing(pairs);
C_Progress_display my_progress_bar( triplets.size(), std::cout,
"Per triplet tracks validation (discard spurious correspondences):\n" );
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel
#endif // OPENMVG_USE_OPENMP
for( Triplets::const_iterator it = triplets.begin(); it != triplets.end(); ++it)
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp single nowait
#endif // OPENMVG_USE_OPENMP
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif // OPENMVG_USE_OPENMP
{++my_progress_bar;}
const graph::Triplet & triplet = *it;
const IndexT I = triplet.i, J = triplet.j , K = triplet.k;
openMVG::tracks::STLMAPTracks map_tracksCommon;
openMVG::tracks::TracksBuilder tracksBuilder;
{
PairWiseMatches map_matchesIJK;
if(putatives_matches.find(std::make_pair(I,J)) != putatives_matches.end())
map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,J)));
if(putatives_matches.find(std::make_pair(I,K)) != putatives_matches.end())
map_matchesIJK.insert(*putatives_matches.find(std::make_pair(I,K)));
if(putatives_matches.find(std::make_pair(J,K)) != putatives_matches.end())
map_matchesIJK.insert(*putatives_matches.find(std::make_pair(J,K)));
if (map_matchesIJK.size() >= 2) {
tracksBuilder.Build(map_matchesIJK);
tracksBuilder.Filter(3);
tracksBuilder.ExportToSTL(map_tracksCommon);
}
// Triangulate the tracks
for (tracks::STLMAPTracks::const_iterator iterTracks = map_tracksCommon.begin();
iterTracks != map_tracksCommon.end(); ++iterTracks) {
{
const tracks::submapTrack & subTrack = iterTracks->second;
Triangulation trianObj;
for (tracks::submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter) {
const size_t imaIndex = iter->first;
const size_t featIndex = iter->second;
const View * view = sfm_data.GetViews().at(imaIndex).get();
const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
const Pose3 pose = sfm_data.GetPoseOrDie(view);
const Vec2 pt = regions_provider->regions_per_view.at(imaIndex)->GetRegionPosition(featIndex);
trianObj.add(cam->get_projective_equivalent(pose), cam->get_ud_pixel(pt));
}
const Vec3 Xs = trianObj.compute();
if (trianObj.minDepth() > 0 && trianObj.error() < 4.0)
// TODO: Add an angular check ?
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif // OPENMVG_USE_OPENMP
{
openMVG::tracks::submapTrack::const_iterator iterI, iterJ, iterK;
iterI = iterJ = iterK = subTrack.begin();
std::advance(iterJ,1);
std::advance(iterK,2);
triplets_matches[std::make_pair(I,J)].push_back(IndMatch(iterI->second, iterJ->second));
triplets_matches[std::make_pair(J,K)].push_back(IndMatch(iterJ->second, iterK->second));
triplets_matches[std::make_pair(I,K)].push_back(IndMatch(iterI->second, iterK->second));
}
}
}
}
}
}
}
// Clear putatives matches since they are no longer required
matching::PairWiseMatches().swap(putatives_matches);
}
void VDBLinearFEMSolverModule<LatticeType>::updateStencil() {
if(!m_obj) {
std::cout << "No object set... shouldn't be updating stencil" << std::endl;
return;
}
const LatticeType& vL = m_obj->getDOFs();
m_vertex_neighbors.resize(3*vL.activeCount());
std::vector<int>::iterator begin=m_vertex_neighbors.begin();
std::vector<int>::iterator end=m_vertex_neighbors.end();
std::fill(begin,end,0);
ActiveLatticeIterator a = m_obj->activeVertexIterator();
int totalReserve = 0;
for(ActiveLatticeIterator active = a; active; ++active) {
for(int32_t m=-1; m < 2; ++m) {
for(int32_t n=-1; n < 2; ++n) {
for(int32_t l=-1; l < 2; ++l) {
//if((m & n & l) == 0)
SIndex3 tmp = active.index() + SIndex3(m,n,l);
//if(!m_obj->validVertexIndex3(tmp)) continue;
if(m_obj->getVertex(tmp.i,tmp.j,tmp.k) != -1) {
m_vertex_neighbors[3*active.value()] += 1;
m_vertex_neighbors[3*active.value()+1] += 1;
m_vertex_neighbors[3*active.value()+2] += 1;
totalReserve+=3;
}
}
}
}
}
int numDOFs = 3*vL.activeCount();
int32_t NI=vL.NI();
int32_t NJ=vL.NJ();
int32_t NK=vL.NK();
std::cout << "Allocating sparse matrix of size: " << numDOFs << std::endl;
M = SparseMatrix(numDOFs,numDOFs);
rhs = Vector::Zero(numDOFs);
//Pre-reserve the places where we will have fillin
M.reserve(m_vertex_neighbors);
typedef Eigen::Triplet<Scalar> Triplet;
typedef std::vector<Triplet> Triplets;
Triplets triplets;
triplets.reserve(totalReserve);
std::vector<bool> used_constraints(numDOFs);
//rms::ScalarGrid::ConstAccessor accessor = GetDistanceGrid()->getConstAccessor();
rms::VectorGridPtr velocityFieldPtr = m_obj->getVectorField();
rms::ScalarGridPtr rigidFieldPtr = m_obj->getRigidField();
rms::ScalarGridPtr heatFieldPtr = m_obj->getHeatField();
rms::RGBAGridPtr materialFieldPtr = m_obj->getMaterialField();
rms::ScalarGrid::ConstAccessor heatAccessor = heatFieldPtr->getConstAccessor();
rms::ScalarGrid::ConstAccessor rigidAccessor = rigidFieldPtr->getConstAccessor();
rms::RGBAGrid::ConstAccessor materialAccessor = materialFieldPtr->getConstAccessor();
rms::VectorGrid::ConstAccessor velocityAccessor = velocityFieldPtr->getConstAccessor();
int count=0;
std::cout << "About to look at heat field" << std::endl;
Scalar scale = heatFieldPtr->transform().voxelVolume();
std::set<int32_t> used_vertex;
StiffnessEntryStorage store(scale,integrator);//TODO: need to fix this
std::cout << "Mincoord: " << m_minCoord << std::endl;
vdb::CoordBBox bbox = m_obj->getDistanceField()->evalActiveVoxelBoundingBox();
std::cout << bbox.min() << " " << bbox.max() << std::endl;
m_minCoord = bbox.min();
for(rms::ScalarGrid::ValueOnCIter it = m_obj->getDistanceField()->cbeginValueOn(); it; ++it) {
if(it.getValue() >= 0) continue;
/*
if(count++ % 1000 == 0) {
std::cout << count << "/" << numDOFs << std::endl;
}
*/
std::cout << "Working in voxel: " << it.getCoord() << std::endl;
Index3 idx = VDBtoLatticeCoord(it.getCoord());
//std::cout << "True index: " << idx << " " << it.getCoord() << std::endl;
/*
if(m_obj->getVertex(idx.i,idx.j,idx.k) == -1) {
std::cout << "bottomleftinner lattice index doesn't exist, clearly lattice just isn't populated"<< std::endl;
}
*/
const vdb::Coord vdbcoord = m_minCoord + vdb::Coord(idx.i,idx.j,idx.k);
vdb::Vec4f material = materialAccessor.getValue(vdbcoord);
vdb::Vec3f velocity = velocityAccessor.getValue(vdbcoord);
float lambda = material(0);
float mu = material(1);
float density = material(3) * 0.125;//Divide because each basis function only takes on 1/8th of a whole cube
for(VDBVoxelNodeIterator<LatticeType> alpha(m_obj->getDOFs(),idx); alpha; ++alpha) {
const Index3 & alpha_index = alpha.index();
const vdb::Coord alpha_vdbcoord = m_minCoord + vdb::Coord(alpha_index.i,alpha_index.j,alpha_index.k);
const int32_t alpha_value = alpha.value();
const int32_t alphaind = mat_ind(alpha_value,0);
//const LinearElasticVertexProperties & alpha_props = m_vertex_properties[alpha_value];
//alpha sets the matrix, betas set the rhs values for alpha
std::cout << alpha_vdbcoord << ": ";
if(rigidAccessor.getValue(alpha_vdbcoord) < 0) {
std::cout << "Rigid!" << std::endl;
if(used_vertex.find(alpha_value) == used_vertex.end()) {
used_vertex.insert(alpha_value);
const int32_t ind = alphaind;
triplets.push_back(Triplet(ind,ind,1));
triplets.push_back(Triplet(ind+1,ind+1,1));
triplets.push_back(Triplet(ind+2,ind+2,1));
std::cout << rhs(alphaind+0) << " ";
std::cout << rhs(alphaind+1) << " ";
std::cout << rhs(alphaind+2) << std::endl;
}
} else {
std::cout << "Elastic!" << std::endl;
rhs(alphaind+0) += velocity(0)*density;
rhs(alphaind+1) += velocity(1)*density;
rhs(alphaind+2) += velocity(2)*density;
//std::cout << velocity << std::endl;
for(VDBVoxelNodeIterator<LatticeType> beta(m_obj->getDOFs(),idx); beta; ++beta) {
const Index3 & beta_index = beta.index();
const int32_t beta_value = beta.value();
/*
if(alpha_value < 0 || beta_value < 0) {
std::cout << "WTF Shouldn't be finding vertices out of nowhere..." << alpha_value << " " << beta_value << ": " << it.getCoord()<< ":A" << alpha_index << ":B" << beta_index << std::endl;
std::cout << idx << " " << it.getCoord() << std::endl;
}*/
//const LinearElasticVertexProperties & beta_props = m_vertex_properties[beta_value];
//const Vector3 beta_externalForce = beta_props.externalForce;
const vdb::Coord beta_vdbcoord = m_minCoord + vdb::Coord(beta_index.i,beta_index.j,beta_index.k);
if(rigidAccessor.getValue(beta_vdbcoord) > 0) {
for(int32_t alphadim = 0; alphadim < 3; ++alphadim) {
const int32_t alpha_i = mat_ind(alpha_value,alphadim);
for(int32_t betadim = 0; betadim < 3; ++betadim) {
const int32_t beta_i = mat_ind(beta_value,betadim);
const Scalar v = store.genericTerm(alpha.pos(), beta.pos(), alphadim, betadim,lambda,mu);
triplets.push_back(Triplet(alpha_i,beta_i,
(
v
)
));
}
}
}
/*
const SIndex3 rel_alpha = alpha_index - idx;
const int32_t ind_alpha = rel_alpha.i * 4 + rel_alpha.j * 2 + rel_alpha.k;
const SIndex3 rel_beta = beta_index - idx;
const int32_t ind_beta = rel_beta.i * 4 + rel_beta.j * 2 + rel_beta.k;
*/
//const Scalar diag = integrator.i(boost::bind(&StiffnessEntryFunctor::diagonalTermi
const Scalar diag = store.diagonalTerm(alpha.pos(), beta.pos(), mu);
for(int32_t dim = 0; dim < 3; ++dim) {
const int32_t ai = mat_ind(alpha_value,dim);
const int32_t bi = mat_ind(beta_value,dim);
triplets.push_back(Triplet(ai,bi,
(
diag
)
));
}
}
}
}
}
//std::cout << rhs.transpose() << std::endl;
std::cout << "Number of matrix etdntries (with duplicates): " << triplets.size() << std::endl;
M.setFromTriplets(triplets.begin(),triplets.end());
M.prune(1,Eigen::NumTraits<Scalar>::dummy_precision());
//std::cout << M << std::endl;
}