39 #if defined(_MSC_VER) || defined(_LIBCPP_VERSION) 42 #include <ext/algorithm> 99 for (
unsigned int i=0; i<
points.size(); i++) {
113 for (
unsigned int i=0; i<
points.size(); i++) {
123 for (
unsigned int i=0; i<
points.size(); i++) {
124 points[i].rotate_IP(roll, pitch, yaw);
130 float min_x, min_y, min_z;
131 float max_x, max_y, max_z;
132 min_x = min_y = min_z = 1e6;
133 max_x = max_y = max_z = -1e6;
143 if (x < min_x) min_x = x;
144 if (y < min_y) min_y = y;
145 if (z < min_z) min_z = z;
147 if (x > max_x) max_x = x;
148 if (y > max_y) max_y = y;
149 if (z > max_z) max_z = z;
152 lowerBound(0) = min_x; lowerBound(1) = min_y; lowerBound(2) = min_z;
153 upperBound(0) = max_x; upperBound(1) = max_y; upperBound(2) = max_z;
161 float min_x, min_y, min_z;
162 float max_x, max_y, max_z;
165 min_x = lowerBound(0); min_y = lowerBound(1); min_z = lowerBound(2);
166 max_x = upperBound(0); max_y = upperBound(1); max_z = upperBound(2);
197 double dist = sqrt(x*x+y*y+z*z);
198 if ( dist > thres ) result.
push_back (x,y,z);
208 #if defined(_MSC_VER) || defined(_LIBCPP_VERSION) 209 samples.reserve(this->
size());
210 samples.insert(samples.end(), this->
begin(), this->
end());
211 std::random_shuffle(samples.begin(), samples.end());
212 samples.resize(num_samples);
214 random_sample_n(
begin(),
end(), std::back_insert_iterator<point3d_collection>(samples), num_samples);
215 for (
unsigned int i=0; i<samples.size(); i++) {
224 std::ofstream outfile (filename.c_str());
226 outfile <<
"#VRML V2.0 utf8" << std::endl;
227 outfile <<
"Transform {" << std::endl;
228 outfile <<
"translation 0 0 0" << std::endl;
229 outfile <<
"rotation 0 0 0 0" << std::endl;
230 outfile <<
" children [" << std::endl;
231 outfile <<
" Shape{" << std::endl;
232 outfile <<
" geometry PointSet {" << std::endl;
233 outfile <<
" coord Coordinate {" << std::endl;
234 outfile <<
" point [" << std::endl;
237 <<
points.size() <<
" points to " 238 << filename.c_str() <<
".");
240 for (
unsigned int i = 0; i < (
points.size()); i++){
241 outfile <<
"\t\t" << (
points[i])(0)
247 outfile <<
" ]" << std::endl;
248 outfile <<
" }" << std::endl;
249 outfile <<
" color Color{" << std::endl;
250 outfile <<
" color [" << std::endl;
252 for (
unsigned int i = 0; i <
points.size(); i++){
253 outfile <<
"\t\t 1.0 1.0 1.0 \n";
256 outfile <<
" ]" << std::endl;
257 outfile <<
" }" << std::endl;
259 outfile <<
" }" << std::endl;
260 outfile <<
" }" << std::endl;
263 outfile <<
" ]" << std::endl;
264 outfile <<
"}" << std::endl;
272 for (
unsigned int i=0; i<3; i++){
287 unsigned int pc_size = 0;
288 s.read((
char*)&pc_size,
sizeof(pc_size));
289 OCTOMAP_DEBUG(
"Reading %d points from binary file...", pc_size);
292 this->
points.reserve(pc_size);
294 for (
unsigned int i=0; i<pc_size; i++) {
313 size_t pc_size = this->
size();
314 OCTOMAP_DEBUG(
"Writing %lu points to binary file...", (
unsigned long)pc_size);
315 s.write((
char*)&pc_size,
sizeof(pc_size));
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
#define OCTOMAP_DEBUG(...)
void minDist(double thres)
void writeVrml(std::string filename)
Export the Pointcloud to a VRML file.
pose6d current_inv_transform
point3d getPoint(unsigned int i) const
void transform(pose6d transform)
Apply transform to each point.
void push_back(float x, float y, float z)
Pose6D inv() const
Inversion.
void subSampleRandom(unsigned int num_samples, Pointcloud &sample_cloud)
std::ostream & writeBinary(std::ostream &s) const
std::vector< octomath::Vector3 > point3d_collection
std::istream & readBinary(std::istream &s)
point3d_collection points
std::istream & readBinary(std::istream &s)
This class represents a tree-dimensional pose of an object.
#define OCTOMAP_WARNING(...)
std::istream & read(std::istream &s)
This class represents a three-dimensional vector.
#define OCTOMAP_DEBUG_STR(args)
void transformAbsolute(pose6d transform)
Apply transform to each point, undo previous transforms.
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
void rotate(double roll, double pitch, double yaw)
Rotate each point in pointcloud.
void calcBBX(point3d &lowerBound, point3d &upperBound) const
Calculate bounding box of Pointcloud.
void crop(point3d lowerBound, point3d upperBound)
Crop Pointcloud to given bounding box.
#define OCTOMAP_ERROR(...)
point3d_collection::const_iterator const_iterator