void ObservationDirectionDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
const int dim(cloud.features.rows() - 1);
const int featDim(cloud.features.cols());
if (dim != 2 && dim != 3)
{
throw InvalidField(
(boost::format("ObservationDirectionDataPointsFilter: Error, works only in 2 or 3 dimensions, cloud has %1% dimensions.") % dim).str()
);
}
Vector center(dim);
center[0] = centerX;
center[1] = centerY;
if (dim == 3)
center[2] = centerZ;
cloud.allocateDescriptor("observationDirections", dim);
BOOST_AUTO(observationDirections, cloud.getDescriptorViewByName("observationDirections"));
for (int i = 0; i < featDim; ++i)
{
// Check normal orientation
const Vector p(cloud.features.block(0, i, dim, 1));
observationDirections.col(i) = center - p;
}
}
//-------------------------------------------------------------------------------------------------
void MessageBase::print(ostream& os, int depth) const
{
const string dspacer((depth + 1) * 3, ' ');
const BaseMsgEntry *tbme(_ctx._bme.find_ptr(_msgType.c_str()));
if (tbme)
os << tbme->_name << " (\"" << _msgType << "\")" << endl;
for (Positions::const_iterator itr(_pos.begin()); itr != _pos.end(); ++itr)
{
const BaseEntry *tbe(_ctx.find_be(itr->second->_fnum));
if (!tbe)
throw InvalidField(itr->second->_fnum);
os << dspacer << tbe->_name;
const unsigned short comp(_fp.getComp(itr->second->_fnum));
if (comp)
os << " [" << _ctx._cn[comp] << ']';
os << " (" << itr->second->_fnum << "): ";
int idx;
if (itr->second->_rlm && (idx = (itr->second->get_rlm_idx())) >= 0)
os << itr->second->_rlm->_descriptions[idx] << " (" << *itr->second << ')' << endl;
else
os << *itr->second << endl;
if (_fp.is_group(itr->second->_fnum))
print_group(itr->second->_fnum, os, depth);
}
}
//-------------------------------------------------------------------------------------------------
unsigned MessageBase::decode_group(const unsigned short fnum, const f8String& from, const unsigned offset)
{
unsigned s_offset(offset), result;
GroupBase *grpbase(find_group(fnum));
if (!grpbase)
throw InvalidRepeatingGroup(fnum);
const unsigned fsize(from.size());
const char *dptr(from.data());
char tag[MAX_FLD_LENGTH], val[MAX_FLD_LENGTH];
for (bool ok(true); ok && s_offset < fsize; )
{
scoped_ptr<MessageBase> grp(grpbase->create_group());
for (unsigned pos(0); s_offset < fsize && (result = extract_element(dptr + s_offset, fsize - s_offset, tag, val));)
{
const unsigned tv(fast_atoi<unsigned>(tag));
Presence::const_iterator itr(grp->_fp.get_presence().end());
if (grp->_fp.get(tv, itr, FieldTrait::present)) // already present; next group?
break;
if (pos == 0 && grp->_fp.getPos(tv, itr) != 1) // first field in group is mandatory
throw MissingRepeatingGroupField(tv);
const BaseEntry *be(_ctx._be.find_ptr(tv));
if (!be)
throw InvalidField(tv);
if (!grp->_fp.has(tv, itr)) // field not found in sub-group - end of repeats?
{
ok = false;
break;
}
s_offset += result;
grp->add_field(tv, itr, ++pos, be->_create(val, be->_rlm, -1), false);
grp->_fp.set(tv, itr, FieldTrait::present); // is present
if (grp->_fp.is_group(tv, itr)) // nested group
s_offset = grp->decode_group(tv, from, s_offset);
}
const unsigned short missing(grp->_fp.find_missing());
if (missing)
{
const BaseEntry& tbe(_ctx._be.find_ref(missing));
ostringstream ostr;
ostr << tbe._name << " (" << missing << ')';
throw MissingMandatoryField(ostr.str());
}
*grpbase += grp.release();
}
return s_offset;
}
//-------------------------------------------------------------------------------------------------
unsigned MessageBase::decode(const f8String& from, const unsigned offset)
{
unsigned s_offset(offset), result;
const unsigned fsize(from.size());
const char *dptr(from.data());
char tag[MAX_FLD_LENGTH], val[MAX_FLD_LENGTH];
for (unsigned pos(_pos.size()); s_offset <= fsize && (result = extract_element(dptr + s_offset, fsize - s_offset, tag, val));)
{
const unsigned tv(fast_atoi<unsigned>(tag));
const BaseEntry *be(_ctx._be.find_ptr(tv));
#if defined PERMIT_CUSTOM_FIELDS
if (!be && (!_ctx._ube || (be = _ctx._ube->find_ptr(tv)) == 0))
#else
if (!be)
#endif
throw InvalidField(tv);
Presence::const_iterator itr(_fp.get_presence().end());
if (!_fp.has(tv, itr))
break;
s_offset += result;
if (_fp.get(tv, itr, FieldTrait::present))
{
if (!_fp.get(tv, itr, FieldTrait::automatic))
throw DuplicateField(tv);
}
else
{
add_field(tv, itr, ++pos, be->_create(val, be->_rlm, -1), false);
if (_fp.is_group(tv, itr))
s_offset = decode_group(tv, from, s_offset);
}
}
const unsigned short missing(_fp.find_missing());
if (missing)
{
const BaseEntry& tbe(_ctx._be.find_ref(missing));
ostringstream ostr;
ostr << tbe._name << " (" << missing << ')';
throw MissingMandatoryField(ostr.str());
}
return s_offset;
}
//-------------------------------------------------------------------------------------------------
void MessageBase::print_field(const unsigned short fnum, ostream& os) const
{
Fields::const_iterator fitr(_fields.find(fnum));
if (fitr != _fields.end())
{
const BaseEntry *tbe(_ctx.find_be(fnum));
if (!tbe)
throw InvalidField(fnum);
os << tbe->_name << " (" << fnum << "): ";
int idx;
if (fitr->second->_rlm && (idx = (fitr->second->get_rlm_idx())) >= 0)
os << fitr->second->_rlm->_descriptions[idx] << " (" << *fitr->second << ')';
else
os << *fitr->second;
if (_fp.is_group(fnum))
print_group(fnum, os, 0);
}
}
//-------------------------------------------------------------------------------------------------
void MessageBase::print(ostream& os, int depth) const
{
const string dspacer((depth + 1) * 3, ' ');
const BaseMsgEntry *tbme(_ctx._bme.find_ptr(_msgType));
if (tbme)
os << tbme->_name << " (\"" << _msgType << "\")" << endl;
for (Positions::const_iterator itr(_pos.begin()); itr != _pos.end(); ++itr)
{
const BaseEntry *tbe(_ctx._be.find_ptr(itr->second->_fnum));
if (!tbe)
#if defined PERMIT_CUSTOM_FIELDS
if (!_ctx._ube || (tbe = _ctx._ube->find_ptr(itr->second->_fnum)) == 0)
#endif
throw InvalidField(itr->second->_fnum);
os << dspacer << tbe->_name << " (" << itr->second->_fnum << "): ";
int idx;
if (itr->second->_rlm && (idx = (itr->second->get_rlm_idx())) >= 0)
os << itr->second->_rlm->_descriptions[idx] << " (" << *itr->second << ')' << endl;
else
os << *itr->second << endl;
if (_fp.is_group(itr->second->_fnum))
print_group(itr->second->_fnum, os, depth);
}
}
void CovarianceSamplingDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
const std::size_t featDim(cloud.features.rows());
assert(featDim == 4); //3D pts only
//Check number of points
const std::size_t nbPoints = cloud.getNbPoints();
if(nbSample >= nbPoints)
return;
//Check if there is normals info
if (!cloud.descriptorExists("normals"))
throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find normals in descriptors.");
const auto& normals = cloud.getDescriptorViewByName("normals");
std::vector<std::size_t> keepIndexes;
keepIndexes.resize(nbSample);
///---- Part A, as we compare the cloud with himself, the overlap is 100%, so we keep all points
//A.1 and A.2 - Compute candidates
std::vector<std::size_t> candidates ;
candidates.resize(nbPoints);
for (std::size_t i = 0; i < nbPoints; ++i) candidates[i] = i;
const std::size_t nbCandidates = candidates.size();
//Compute centroid
Vector3 center;
for(std::size_t i = 0; i < featDim - 1; ++i) center(i) = T(0.);
for (std::size_t i = 0; i < nbCandidates; ++i)
for (std::size_t f = 0; f <= 3; ++f)
center(f) += cloud.features(f,candidates[i]);
for(std::size_t i = 0; i <= 3; ++i) center(i) /= T(nbCandidates);
//Compute torque normalization
T Lnorm = 1.0;
if(normalizationMethod == TorqueNormMethod::L1)
{
Lnorm = 1.0;
}
else if(normalizationMethod == TorqueNormMethod::Lavg)
{
Lnorm = 0.0;
for (std::size_t i = 0; i < nbCandidates; ++i)
Lnorm += (cloud.features.col(candidates[i]).head(3) - center).norm();
Lnorm /= nbCandidates;
}
else if(normalizationMethod == TorqueNormMethod::Lmax)
{
const Vector minValues = cloud.features.rowwise().minCoeff();
const Vector maxValues = cloud.features.rowwise().maxCoeff();
const Vector3 radii = maxValues.head(3) - minValues.head(3);
Lnorm = radii.maxCoeff() / 2.; //radii.mean() / 2.;
}
//A.3 - Compute 6x6 covariance matrix + EigenVectors
auto computeCovariance = [Lnorm, nbCandidates, &cloud, ¢er, &normals, &candidates](Matrix66 & cov) -> void {
//Compute F matrix, see Eq. (4)
Eigen::Matrix<T, 6, Eigen::Dynamic> F(6, nbCandidates);
for(std::size_t i = 0; i < nbCandidates; ++i)
{
const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c
const Vector3 ni = normals.col(candidates[i]).head(3);
//compute (1 / L) * (pi - c) x ni
F.template block<3, 1>(0, i) = (1. / Lnorm) * p.cross(ni);
//set ni part
F.template block<3, 1>(3, i) = ni;
}
// Compute the covariance matrix Cov = FF'
cov = F * F.transpose();
};
Matrix66 covariance;
computeCovariance(covariance);
Eigen::EigenSolver<Matrix66> solver(covariance);
const Matrix66 eigenVe = solver.eigenvectors().real();
const Vector6 eigenVa = solver.eigenvalues().real();
///---- Part B
//B.1 - Compute the v-6 for each candidate
std::vector<Vector6, Eigen::aligned_allocator<Vector6>> v; // v[i] = [(pi-c) x ni ; ni ]'
v.resize(nbCandidates);
for(std::size_t i = 0; i < nbCandidates; ++i)
{
const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c
const Vector3 ni = normals.col(candidates[i]).head(3);
v[i].template block<3, 1>(0, 0) = (1. / Lnorm) * p.cross(ni);
v[i].template block<3, 1>(3, 0) = ni;
}
//B.2 - Compute the 6 sorted list based on dot product (vi . Xk) = magnitude, with Xk the kth-EigenVector
std::vector<std::list<std::pair<int, T>>> L; // contain list of pair (index, magnitude) contribution to the eigens vectors
L.resize(6);
//sort by decreasing magnitude
auto comp = [](const std::pair<int, T>& p1, const std::pair<int, T>& p2) -> bool {
return p1.second > p2.second;
};
for(std::size_t k = 0; k < 6; ++k)
{
for(std::size_t i = 0; i < nbCandidates; ++i )
{
L[k].push_back(std::make_pair(i, std::fabs( v[i].dot(eigenVe.template block<6,1>(0, k)) )));
}
L[k].sort(comp);
}
std::vector<T> t(6, T(0.)); //contains the sums of squared magnitudes
std::vector<bool> sampledPoints(nbCandidates, false); //maintain flag to avoid resampling the same point in an other list
///Add point iteratively till we got the desired number of point
for(std::size_t i = 0; i < nbSample; ++i)
{
//B.3 - Equally constrained all eigen vectors
// magnitude contribute to t_i where i is the indice of th least contrained eigen vector
//Find least constrained vector
std::size_t k = 0;
for (std::size_t i = 0; i < 6; ++i)
{
if (t[k] > t[i])
k = i;
}
// Add the point from the top of the list corresponding to the dimension to the set of samples
while(sampledPoints[L[k].front().first])
L[k].pop_front(); //remove already sampled point
//Get index to keep
const std::size_t idToKeep = static_cast<std::size_t>(L[k].front().first);
L[k].pop_front();
sampledPoints[idToKeep] = true; //set flag to avoid resampling
//B.4 - Update the running total
for (std::size_t k = 0; k < 6; ++k)
{
const T magnitude = v[idToKeep].dot(eigenVe.template block<6, 1>(0, k));
t[k] += (magnitude * magnitude);
}
keepIndexes[i] = candidates[idToKeep];
}
//TODO: evaluate performances between this solution and sorting the indexes
// We build map of (old index to new index), in case we sample pts at the begining of the pointcloud
std::unordered_map<std::size_t, std::size_t> mapidx;
std::size_t idx = 0;
///(4) Sample the point cloud
for(std::size_t id : keepIndexes)
{
//retrieve index from lookup table if sampling in already switched element
if(id<idx)
id = mapidx[id];
//Switch columns id and idx
cloud.swapCols(idx, id);
//Maintain new index position
mapidx[idx] = id;
//Update index
++idx;
}
cloud.conservativeResize(nbSample);
}
