shape_cluster.cpp
Go to the documentation of this file.00001 00059 #include "cob_3d_mapping_common/shape_cluster.h" 00060 #include <iostream> 00061 00062 void 00063 cob_3d_mapping::ShapeCluster::computeAttributes() 00064 { 00065 // TODO: proper computation of bounding box size 00066 scale = Eigen::Vector3f(0.1,0.1,0.1); 00067 this->centroid = Eigen::Vector4f::Zero(); 00068 int i = 0; 00069 for(std::list<Shape::Ptr>::iterator it=shapes.begin(); it!=shapes.end(); ++it) 00070 { 00071 this->centroid += (*it)->centroid; 00072 ++i; 00073 } 00074 this->centroid /= static_cast<float>(i); 00075 } 00076 00077 void 00078 cob_3d_mapping::ShapeCluster::transform2tf(const Eigen::Affine3f& trafo) 00079 { 00080 for(std::list<Shape::Ptr>::iterator i=shapes.begin(); i!=shapes.end(); ++i) (*i)->transform2tf(trafo); 00081 } 00082 00083 void 00084 cob_3d_mapping::ShapeCluster::getMergeCandidates(const std::vector<ShapeCluster::Ptr>& sc_vec, 00085 std::vector<int>& intersections) const 00086 { 00087 for(size_t i=0; i<sc_vec.size(); ++i) 00088 { 00089 //float length = (this->centroid.head(3) - sc_vec[i]->centroid.head(3)).norm(); 00090 //std::cout << length << std::endl; 00091 if ( this->hasSimilarParametersWith(sc_vec[i]) ) intersections.push_back(i); 00092 } 00093 } 00094 00095 void 00096 cob_3d_mapping::ShapeCluster::merge(std::vector<ShapeCluster::Ptr>&sc_vec) 00097 { 00098 int new_merged = 1; 00099 for(size_t i=0; i<sc_vec.size(); ++i) 00100 { 00101 this->shapes.insert(this->shapes.end(), sc_vec[i]->shapes.begin(), sc_vec[i]->shapes.end()); 00102 new_merged += sc_vec[i]->merged; 00103 } 00104 this->merged = new_merged; 00105 this->computeAttributes(); 00106 } 00107 00108 void 00109 cob_3d_mapping::ShapeCluster::insert(Shape::Ptr shape) 00110 { 00111 shapes.push_back(shape); 00112 }