/**
* Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
*/
void basis_splines_endreps_local_v2(float *knots, int n_knots, int order, int *boundaries, int n_boundaries, Eigen::MatrixXf &Xt) {
assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
int n_cols = n_knots + order; // number of basis vectors we'll have at end
Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
Xt.fill(0);
int n_std_knots = n_knots + 2 * order;
float std_knots[n_std_knots];
// Repeat the boundary knots on there to ensure linear out side of knots
for (int i = 0; i < order; i++) {
std_knots[i] = boundaries[0];
}
// Our original boundary knots here
for (int i = 0; i < n_knots; i++) {
std_knots[i+order] = knots[i];
}
// Repeat the boundary knots on there to ensure linear out side of knots
for (int i = 0; i < order; i++) {
std_knots[i+order+n_knots] = boundaries[1];
}
// Evaluate our basis splines at each frame.
for (int i = boundaries[0]; i < boundaries[1]; i++) {
int idx = -1;
// find index such that i >= knots[idx] && i < knots[idx+1]
float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
idx = val - std_knots - 1;
assert(idx >= 0);
float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
basis_spline_xi_v2(std_knots, n_std_knots, order, idx, i, f);
}
// Put in our conventional format where each column is a basis vector
Xt.transposeInPlace();
}
void PixelMapper::mergeProjections(Eigen::MatrixXf& depthImage, Eigen::MatrixXi& indexImage,
Eigen::MatrixXf* depths, Eigen::MatrixXi* indices, int numImages){
assert (numImages>0);
int rows=depths[0].rows();
int cols=depths[0].cols();
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.resize(rows, cols);
indexImage.fill(-1);
#pragma omp parallel for
for (int c=0; c<cols; c++){
int* destIndexPtr = &indexImage.coeffRef(0,c);
float* destDepthPtr = &depthImage.coeffRef(0,c);
int* srcIndexPtr[numImages];
float* srcDepthPtr[numImages];
for (int i=0; i<numImages; i++){
srcIndexPtr[i] = &indices[i].coeffRef(0,c);
srcDepthPtr[i] = &depths[i].coeffRef(0,c);
}
for (int r=0; r<rows; r++){
for (int i=0; i<numImages; i++){
if (*destDepthPtr>*srcDepthPtr[i]){
*destDepthPtr = *srcDepthPtr[i];
*destIndexPtr = *srcIndexPtr[i];
}
srcDepthPtr[i]++;
srcIndexPtr[i]++;
}
destDepthPtr++;
destIndexPtr++;
}
}
}
void DenseCRF::stepInference( Eigen::MatrixXf & Q, Eigen::MatrixXf & tmp1, Eigen::MatrixXf & tmp2 ) const{
tmp1.resize( Q.rows(), Q.cols() );
tmp1.fill(0);
if( unary_ )
tmp1 -= unary_->get();
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q );
tmp1 -= tmp2;
}
// Exponentiate and normalize
expAndNormalize( Q, tmp1 );
}
void PointProjector::project(Eigen::MatrixXi &indexImage,
Eigen::MatrixXf &depthImage,
const HomogeneousPoint3fVector &points) const {
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.fill(-1);
const HomogeneousPoint3f* point = &points[0];
for (size_t i=0; i<points.size(); i++, point++){
int x, y;
float d;
if (!project(x, y, d, *point) ||
x<0 || x>=indexImage.rows() ||
y<0 || y>=indexImage.cols() )
continue;
float& otherDistance=depthImage(x,y);
int& otherIndex=indexImage(x,y);
if (otherDistance>d) {
otherDistance = d;
otherIndex = i;
}
}
}
void Match
(
const sfm::SfM_Data & sfm_data,
const sfm::Regions_Provider & regions_provider,
const Pair_Set & pairs,
float fDistRatio,
PairWiseMatchesContainer & map_PutativesMatches // the pairwise photometric corresponding points
)
{
C_Progress_display my_progress_bar( pairs.size() );
// Collect used view indexes
std::set<IndexT> used_index;
// Sort pairs according the first index to minimize later memory swapping
using Map_vectorT = std::map<IndexT, std::vector<IndexT>>;
Map_vectorT map_Pairs;
for (Pair_Set::const_iterator iter = pairs.begin(); iter != pairs.end(); ++iter)
{
map_Pairs[iter->first].push_back(iter->second);
used_index.insert(iter->first);
used_index.insert(iter->second);
}
using BaseMat = Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
// Init the cascade hasher
CascadeHasher cascade_hasher;
if (!used_index.empty())
{
const IndexT I = *used_index.begin();
std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
const size_t dimension = regionsI.get()->DescriptorLength();
cascade_hasher.Init(dimension);
}
std::map<IndexT, HashedDescriptions> hashed_base_;
// Compute the zero mean descriptor that will be used for hashing (one for all the image regions)
Eigen::VectorXf zero_mean_descriptor;
{
Eigen::MatrixXf matForZeroMean;
for (int i =0; i < used_index.size(); ++i)
{
std::set<IndexT>::const_iterator iter = used_index.begin();
std::advance(iter, i);
const IndexT I = *iter;
std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
const ScalarT * tabI =
reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
const size_t dimension = regionsI.get()->DescriptorLength();
if (i==0)
{
matForZeroMean.resize(used_index.size(), dimension);
matForZeroMean.fill(0.0f);
}
if (regionsI.get()->RegionCount() > 0)
{
Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);
matForZeroMean.row(i) = CascadeHasher::GetZeroMeanDescriptor(mat_I);
}
}
zero_mean_descriptor = CascadeHasher::GetZeroMeanDescriptor(matForZeroMean);
}
// Index the input regions
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
for (int i =0; i < used_index.size(); ++i)
{
std::set<IndexT>::const_iterator iter = used_index.begin();
std::advance(iter, i);
const IndexT I = *iter;
std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
const ScalarT * tabI =
reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
const size_t dimension = regionsI.get()->DescriptorLength();
Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);
HashedDescriptions hashed_description = cascade_hasher.CreateHashedDescriptions(mat_I,
zero_mean_descriptor);
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
{
hashed_base_[I] = std::move(hashed_description);
}
}
// Perform matching between all the pairs
for (Map_vectorT::const_iterator iter = map_Pairs.begin();
iter != map_Pairs.end(); ++iter)
{
const IndexT I = iter->first;
const std::vector<IndexT> & indexToCompare = iter->second;
std::shared_ptr<features::Regions> regionsI = regions_provider.get(I);
if (regionsI.get()->RegionCount() == 0)
{
my_progress_bar += indexToCompare.size();
continue;
}
const std::vector<features::PointFeature> pointFeaturesI = regionsI.get()->GetRegionsPositions();
const ScalarT * tabI =
reinterpret_cast<const ScalarT*>(regionsI.get()->DescriptorRawData());
const size_t dimension = regionsI.get()->DescriptorLength();
Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.get()->RegionCount(), dimension);
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
for (int j = 0; j < (int)indexToCompare.size(); ++j)
{
size_t J = indexToCompare[j];
std::shared_ptr<features::Regions> regionsJ = regions_provider.get(J);
if (regionsI.get()->Type_id() != regionsJ.get()->Type_id())
{
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
++my_progress_bar;
continue;
}
// Matrix representation of the query input data;
const ScalarT * tabJ = reinterpret_cast<const ScalarT*>(regionsJ.get()->DescriptorRawData());
Eigen::Map<BaseMat> mat_J( (ScalarT*)tabJ, regionsJ.get()->RegionCount(), dimension);
IndMatches pvec_indices;
using ResultType = typename Accumulator<ScalarT>::Type;
std::vector<ResultType> pvec_distances;
pvec_distances.reserve(regionsJ.get()->RegionCount() * 2);
pvec_indices.reserve(regionsJ.get()->RegionCount() * 2);
// Match the query descriptors to the database
cascade_hasher.Match_HashedDescriptions<BaseMat, ResultType>(
hashed_base_[J], mat_J,
hashed_base_[I], mat_I,
&pvec_indices, &pvec_distances);
std::vector<int> vec_nn_ratio_idx;
// Filter the matches using a distance ratio test:
// The probability that a match is correct is determined by taking
// the ratio of distance from the closest neighbor to the distance
// of the second closest.
matching::NNdistanceRatio(
pvec_distances.begin(), // distance start
pvec_distances.end(), // distance end
2, // Number of neighbor in iterator sequence (minimum required 2)
vec_nn_ratio_idx, // output (indices that respect the distance Ratio)
Square(fDistRatio));
matching::IndMatches vec_putative_matches;
vec_putative_matches.reserve(vec_nn_ratio_idx.size());
for (size_t k=0; k < vec_nn_ratio_idx.size(); ++k)
{
const size_t index = vec_nn_ratio_idx[k];
vec_putative_matches.emplace_back(pvec_indices[index*2].j_, pvec_indices[index*2].i_);
}
// Remove duplicates
matching::IndMatch::getDeduplicated(vec_putative_matches);
// Remove matches that have the same (X,Y) coordinates
const std::vector<features::PointFeature> pointFeaturesJ = regionsJ.get()->GetRegionsPositions();
matching::IndMatchDecorator<float> matchDeduplicator(vec_putative_matches,
pointFeaturesI, pointFeaturesJ);
matchDeduplicator.getDeduplicated(vec_putative_matches);
#ifdef OPENMVG_USE_OPENMP
#pragma omp critical
#endif
{
++my_progress_bar;
if (!vec_putative_matches.empty())
{
map_PutativesMatches.insert( make_pair( make_pair(I,J), std::move(vec_putative_matches) ));
}
}
}
}
}