void MeanShift::find_index_to_merge(arma::colvec3& c,arma::mat& points, double threashod){
std::vector<std::size_t> index_keep;
std::vector<std::size_t> index_merge;
for(std::size_t i = 0; i < points.n_rows;i++){
if(arma::norm(c - points.row(i).st(),2) < threashod){
index_merge.push_back(i);
}else{
index_keep.push_back(i);
}
}
if(index_merge.size() != 0){
arma::colvec3 new_point;
arma::mat new_points(index_keep.size()+1,3);
for(std::size_t i = 0; i < index_merge.size();i++){
new_point = new_point + points.row(index_merge[i]).st();
}
for(std::size_t i = 0; i < index_keep.size();i++){
new_points.row(i) = points.row(index_keep[i]);
}
new_points.row(index_keep.size()) = new_point.st();
points = new_points;
}
}
std::vector<GeoLib::Point*>* PointVec::getSubset(const std::vector<size_t> &subset)
{
std::vector<GeoLib::Point*> *new_points (new std::vector<GeoLib::Point*>(subset.size()));
const size_t nPoints(subset.size());
for (size_t i = 0; i < nPoints; i++)
(*new_points)[i] = new GeoLib::PointWithID((*this->_data_vec)[subset[i]]->getCoords(), subset[i]);
return new_points;
}
std::vector<GEOLIB::Point*>* PointVec::deepcopy(
const std::vector<GEOLIB::Point*>* pnt_vec)
{
std::vector<GEOLIB::Point*>* new_points(new std::vector<GEOLIB::Point*>);
const size_t nPoints(pnt_vec->size());
for (size_t i = 0; i < nPoints; i++)
new_points->push_back(new GEOLIB::Point((*pnt_vec)[i]->getData()));
return new_points;
}
void MeshDenoisingBase::updateVertexPosition(TriMesh &mesh, std::vector<TriMesh::Normal> &filtered_normals, int iteration_number, bool fixed_boundary)
{
std::vector<TriMesh::Point> new_points(mesh.n_vertices());
std::vector<TriMesh::Point> centroid;
for(int iter = 0; iter < iteration_number; iter++)
{
getFaceCentroid(mesh, centroid);
for(TriMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); v_it++)
{
TriMesh::Point p = mesh.point(*v_it);
if(fixed_boundary && mesh.is_boundary(*v_it))
{
new_points.at(v_it->idx()) = p;
}
else
{
double face_num = 0.0;
TriMesh::Point temp_point(0.0, 0.0, 0.0);
for(TriMesh::VertexFaceIter vf_it = mesh.vf_iter(*v_it); vf_it.is_valid(); vf_it++)
{
TriMesh::Normal temp_normal = filtered_normals[vf_it->idx()];
TriMesh::Point temp_centroid = centroid[vf_it->idx()];
temp_point += temp_normal * (temp_normal | (temp_centroid - p));
face_num++;
}
p += temp_point / face_num;
new_points.at(v_it->idx()) = p;
}
}
for(TriMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); v_it++)
mesh.set_point(*v_it, new_points[v_it->idx()]);
}
}
void Proc3DSCropper::update_poses(const std::vector<cv::KeyPoint> &points) {
if (this->poses.size() == 0) {
++this->count;
for (const cv::KeyPoint &kp : points) {
this->poses.push_back(kp.pt);
this->sum_po2_distances.push_back(0);
this->sum_distances.push_back(0);
this->stddevs.push_back(0);
for(int i = 0; i < this->color_num; ++i) {
this->distances.push_back(std::vector<double>());
}
}
}
else if (this->count <= lean_count) {
++this->count;
std::cout << "learning..." << std::endl;
this->poses.clear();
for (const cv::KeyPoint &kp : points) {
this->poses.push_back(kp.pt);
}
bool to_update[this->color_num] = {false};
this->update_stats(points, to_update);
}
else {
std::vector<cv::KeyPoint> new_points(this->color_num);
this->sort_by_distance_from_poses(points, new_points);
bool to_update[this->color_num] = {false};
this->update_stats(new_points, to_update);
for (int index = 0; index < this->color_num; ++index) {
if (to_update[index]) {
this->poses[index] = new_points[index].pt;
}
}
}
this->sort_rotate();
};