Skip to content

Commit

Permalink
Merge pull request #33 from facontidavide/master
Browse files Browse the repository at this point in the history 
Speedup (reducing memory allocations)
  • Loading branch information
@akgoins
akgoins authored Nov 22, 2016
2 parents 797d03f + 6229aeb commit 09959b8
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 19 deletions.
49 changes: 30 additions & 19 deletions map_creator/src/create_reachability_map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,9 @@ using namespace kinematics;

struct stat st;

typedef vector<std::pair< vector< double >, const vector< double >* > > MultiVector;
typedef multimap< const vector< double >*, const vector< double >* > MultiMap;

bool isFloat(string s)
{
istringstream iss(s);
Expand Down Expand Up @@ -126,22 +129,24 @@ int main(int argc, char **argv)
// If the resolution is 0.01 the programs not responds

float radius = resolution;
vector< geometry_msgs::PoseArray > pose_Col;
vector< vector< double > > SphereCoord;
SphereCoord.resize( newData.size() );

MultiVector PoseCol;
PoseCol.reserve( newData.size() * 50);

multimap< vector< double >, vector< double > > PoseCol;
for (int i = 0; i < newData.size(); i++)
{
vector< geometry_msgs::Pose > pose;
vector< double > sphere_coord;
sd.convertPointToVector(newData[i], sphere_coord);
sd.convertPointToVector(newData[i], SphereCoord[i]);

pose = sd.make_sphere_poses(newData[i], radius);
for (int j = 0; j < pose.size(); j++)
{
vector< double > point_on_sphere;
sd.convertPoseToVector(pose[j], point_on_sphere);

PoseCol.insert(pair< vector< double >, vector< double > >(point_on_sphere, sphere_coord));
PoseCol.push_back( std::make_pair(point_on_sphere, &SphereCoord[i]));
}
}

Expand All @@ -151,16 +156,18 @@ int main(int argc, char **argv)
// TODO Support for more than 6DOF robots needs to be implemented.

// Kinematics k;
multimap< vector< double >, vector< double > > PoseColFilter;
MultiMap PoseColFilter;
vector< vector< double > > ikSolutions;
for (multimap< vector< double >, vector< double > >::iterator it = PoseCol.begin(); it != PoseCol.end(); ++it)
ikSolutions.reserve( PoseCol.size() );

for (MultiVector::iterator it = PoseCol.begin(); it != PoseCol.end(); ++it)
{
std::vector< double > joints;
joints.resize(6);
int solns;
if (k.isIKSuccess(it->first, joints, solns))
{
PoseColFilter.insert(pair< vector< double >, vector< double > >(it->second, it->first));
PoseColFilter.insert( std::make_pair( it->second, &(it->first)));
ikSolutions.push_back(joints);
// cout<<it->first[0]<<" "<<it->first[1]<<" "<<it->first[2]<<" "<<it->first[3]<<" "<<it->first[4]<<"
// "<<it->first[5]<<" "<<it->first[6]<<endl;
Expand All @@ -175,25 +182,29 @@ int main(int argc, char **argv)
// TODO there are several maps are implemented. We can get rid of few maps and run large loops. The complexity of
// accessing map is Olog(n)

map< vector< double >, double > sphereColor;
map< const vector< double >*, double > sphereColor;
vector< vector< double > > poseReach;

for (multimap< vector< double >, vector< double > >::iterator it = PoseColFilter.begin(); it != PoseColFilter.end();
++it)
for (MultiMap::iterator it = PoseColFilter.begin(); it != PoseColFilter.end(); ++it)
{
const vector<double>* sphere_coord = it->first;
const vector<double>* point_on_sphere = it->second;

// Reachability Index D=R/N*100;
float d = float(PoseColFilter.count(it->first)) / (PoseCol.size() / newData.size()) * 100;
sphereColor.insert(pair< vector< double >, double >(it->first, double(d)));
float d = float(PoseColFilter.count(sphere_coord)) / (PoseCol.size() / newData.size()) * 100;
sphereColor.insert( std::make_pair(it->first, double(d)));

// poseReach.push_back(it->second);
vector< double > poseAndSphere;
for (int i = 0; i < (it->first).size(); i++)
poseAndSphere.reserve( sphere_coord->size() + point_on_sphere->size());

for (int i = 0; i < sphere_coord->size(); i++)
{
poseAndSphere.push_back((it->first)[i]);
poseAndSphere.push_back((*it->first)[i]);
}
for (int j = 0; j < (it->second).size(); j++)
for (int j = 0; j < point_on_sphere->size(); j++)
{
poseAndSphere.push_back((it->second)[j]);
poseAndSphere.push_back((*it->second)[j]);
}

poseReach.push_back(poseAndSphere);
Expand Down Expand Up @@ -381,11 +392,11 @@ int main(int argc, char **argv)
dims2[1] = SY;
double dset2_data[SX][SY];

for (map< vector< double >, double >::iterator it = sphereColor.begin(); it != sphereColor.end(); ++it)
for (map< const vector< double >*, double >::iterator it = sphereColor.begin(); it != sphereColor.end(); ++it)
{
for (int j = 0; j < SY - 1; j++)
{
dset2_data[distance(sphereColor.begin(), it)][j] = it->first[j];
dset2_data[distance(sphereColor.begin(), it)][j] = (*it->first)[j];
}
for (int j = 3; j < SY; j++)
{
Expand Down
15 changes: 15 additions & 0 deletions map_creator/src/sphere_discretization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ OcTree* SphereDiscretization::generateSphereTree2(point3d origin, float radius,
unsigned sphere_beams = 500;
double angle = 2.0 * M_PI / double(sphere_beams);
Pointcloud p;
p.reserve(sphere_beams*sphere_beams);
for (unsigned i = 0; i < sphere_beams; i++)
{
for (unsigned j = 0; j < sphere_beams; j++)
Expand Down Expand Up @@ -72,6 +73,7 @@ OcTree* SphereDiscretization::generateBoxTree(point3d origin, float diameter, fl
Pointcloud SphereDiscretization::make_sphere_points(point3d origin, double r)
{
Pointcloud spherePoints;
spherePoints.reserve( 7*7*2 );
for (double phi = 0.; phi < 2 * M_PI; phi += M_PI / 7.) // Azimuth [0, 2PI]
{
for (double theta = 0.; theta < M_PI; theta += M_PI / 7.) // Elevation [0, PI]
Expand All @@ -89,6 +91,7 @@ Pointcloud SphereDiscretization::make_sphere_points(point3d origin, double r)
vector< geometry_msgs::Pose > SphereDiscretization::make_sphere_poses(point3d origin, double r)
{
vector< geometry_msgs::Pose > pose_Col;
pose_Col.reserve( 5*5*2 );
geometry_msgs::Pose pose;
// TODO Most of the robots have a roll joint as their final joint which can move 0 to 2pi. So if a pose is reachable,
// then the discretization of roll poses are also reachable. It will increase the data, so we have to decide if we
Expand Down Expand Up @@ -127,6 +130,7 @@ double SphereDiscretization::irand(int min, int max)
Pointcloud SphereDiscretization::make_sphere_rand(point3d origin, double r, int sample)
{
Pointcloud spherePoints;
spherePoints.reserve(sample);
double theta = 0, phi = 0;
for (int i = 0; i < sample; i++)
{
Expand All @@ -145,6 +149,7 @@ Pointcloud SphereDiscretization::make_sphere_rand(point3d origin, double r, int
Pointcloud SphereDiscretization::make_sphere_Archimedes(point3d origin, double r, int sample)
{
Pointcloud spherePoints;
spherePoints.reserve(sample*2);
double theta = 0, phi = 0;
for (int i = 0; i < sample / 2; i++)
{
Expand All @@ -169,6 +174,7 @@ Pointcloud SphereDiscretization::make_sphere_fibonacci_grid(point3d origin, doub
double ng;
double r_phi = (1.0 + sqrt(5.0)) / 2.0;
Pointcloud spherePoints;
spherePoints.reserve(sample*2);
ng = double(sample);
double theta = 0, phi = 0;
for (int i = 0; i < sample; i++)
Expand Down Expand Up @@ -214,6 +220,7 @@ double SphereDiscretization::r8_modp(double x, double y)
Pointcloud SphereDiscretization::make_sphere_spiral_points(point3d origin, double r, int sample)
{
Pointcloud spherePoints;
spherePoints.reserve(sample);

double theta = 0, phi = 0;
double sinphi = 0, cosphi = 0;
Expand Down Expand Up @@ -277,6 +284,7 @@ Pointcloud SphereDiscretization::make_long_lat_grid(point3d origin, double r, in

void SphereDiscretization::convertPointToVector(const point3d point, std::vector< double >& data)
{
data.reserve(3);
data.push_back(double(point.x()));
data.push_back(double(point.y()));
data.push_back(double(point.z()));
Expand All @@ -291,6 +299,7 @@ void SphereDiscretization::convertVectorToPoint(const std::vector< double > data

void SphereDiscretization::convertPoseToVector(const geometry_msgs::Pose pose, std::vector< double >& data)
{
data.reserve(7);
data.push_back(double(pose.position.x));
data.push_back(double(pose.position.y));
data.push_back(double(pose.position.z));
Expand Down Expand Up @@ -484,6 +493,7 @@ void SphereDiscretization::findOptimalPosebyAverage(const vector< geometry_msgs:
double totalVecX, totalVecY, totalVecZ;
double avgVecX, avgVecY, avgVecZ;
vector< tf2::Quaternion > quatCol;
quatCol.reserve(probBasePoses.size());
for (int i = 0; i < probBasePoses.size(); ++i)
{
totalVecX += probBasePoses[i].position.x;
Expand Down Expand Up @@ -555,6 +565,7 @@ void SphereDiscretization::associatePose(multimap< vector< double >, vector< dou
// create a point cloud which consists of all of the possible base locations for all grasp poses and a list of base
// pose orientations
vector< pair< vector< float >, vector< float > > > trns_col;
trns_col.reserve(grasp_poses.size());
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud(new pcl::PointCloud< pcl::PointXYZ >);
for (int i = 0; i < grasp_poses.size(); ++i)
{
Expand Down Expand Up @@ -587,10 +598,12 @@ void SphereDiscretization::associatePose(multimap< vector< double >, vector< dou
new_trans_quat.normalize();

vector< float > position;
position.reserve(3);
position.push_back(new_trans_vec[0]);
position.push_back(new_trans_vec[1]);
position.push_back(new_trans_vec[2]);
vector< float > orientation;
position.reserve(4);
orientation.push_back(new_trans_quat[0]);
orientation.push_back(new_trans_quat[1]);
orientation.push_back(new_trans_quat[2]);
Expand Down Expand Up @@ -624,6 +637,7 @@ void SphereDiscretization::associatePose(multimap< vector< double >, vector< dou
if (pointIdxVec.size() > 0)
{
std::vector< double > voxel_pos;
voxel_pos.reserve(3);
voxel_pos.push_back(searchPoint.x);
voxel_pos.push_back(searchPoint.y);
voxel_pos.push_back(searchPoint.z);
Expand All @@ -633,6 +647,7 @@ void SphereDiscretization::associatePose(multimap< vector< double >, vector< dou
{
// Get the base pose for a given index found in a voxel
vector< double > base_pose;
base_pose.reserve(3);
base_pose.push_back(voxel_pos[0]);
base_pose.push_back(voxel_pos[1]);
base_pose.push_back(voxel_pos[2]);
Expand Down

0 comments on commit 09959b8

Please sign in to comment.