void solve_loop(int* visited, int places){
root[0]=0;
int i;
double this_dis;
if(places == N-2){
for(i=1; i<N; i++){
if(visited[i]==0){
places ++;
root[places]=i;
this_dis=sum_distance(root);
if(this_dis < mindis){
mindis = this_dis;
// for(int j=0; j<N; j++){
// printf("%d ",root[j]);
//}
printf("%lf\n", mindis);
}
places--;
}
}
}else{
for(i=1; i<N; i++){
if(visited[i]==0){
places++;
root[places]=i;
visited[i]=1;
solve_loop(visited, places);
visited[i]=0;
places--;
}
}
}
}
void solve(){
int i;
int visited[N], places=0;
//root[0]=0;
//visited[i]:if i is visited, 1
// :if not, 0
for(i=0; i<N; i++){
visited[i]=0;
root[i]=i;
}
mindis = sum_distance(root);
solve_loop(visited, places);
}
pcl::PointCloud<pcl::PointXYZRGB> KinectFilter::remove_error(Histogram noise)
{
// Only remove Z error for now....
std::vector<float> avg_distance(noise.size(), 0.0), sum_distance(noise.size(), 0.0), number_distance(noise.size(), 0.0);
pcl::PointCloud<pcl::PointXYZRGB> p_out_cloud_ret;
for (size_t i = 0; i < p_in_cloud->size(); i++)
{
Eigen::Vector3f xyz = p_in_cloud->points.at(i).getVector3fMap();
Eigen::Vector3i rgb;
rgb = p_in_cloud->points.at(i).getRGBVector3i();
for (size_t j = 0; j < noise.size(); j++)
{
if (xyz(2) >= noise.at(j).r_min && xyz(2) < noise.at(j).r_max)
{
xyz(2) -= noise.at(j).err;
sum_distance.at(j) += xyz(2);
number_distance.at(j)++;
pcl::PointXYZRGB filtered_p(rgb(0), rgb(1), rgb(2));
filtered_p.getVector3fMap() = xyz;
p_out_cloud_ret.points.push_back(filtered_p);
break;
}
}
}
for (size_t j = 0; j < noise.size(); j++)
{
avg_distance.at(j) = sum_distance.at(j)/number_distance.at(j);
std::cout << "distance " << j << ": " << avg_distance.at(j) << std::endl;
}
return p_out_cloud_ret;
}
