From f7853c48b99d14d78fac41f73233e81708036616 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Wed, 13 Jul 2016 15:31:47 +0200
Subject: [PATCH] [C++] Pre-allocate memory for GeometryData vectors with
reserve instead of resize
---
python/bindings_geometry_object.py | 8 ++++----
src/algorithm/collisions.hpp | 4 ++--
src/multibody/geometry.hpp | 14 +++++++-------
src/multibody/geometry.hxx | 28 ++++++++++++++--------------
src/python/geometry-model.hpp | 6 +++---
src/python/robot_wrapper.py | 4 ++--
unittest/geom.cpp | 4 ++--
7 files changed, 34 insertions(+), 34 deletions(-)
diff --git a/python/bindings_geometry_object.py b/python/bindings_geometry_object.py
index bbb5a8fe8..9374d7187 100644
--- a/python/bindings_geometry_object.py
+++ b/python/bindings_geometry_object.py
@@ -25,13 +25,13 @@ class TestGeometryObjectBindings(unittest.TestCase):
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
def test_name_get_set(self):
- col = self.robot.collision_model.geometry_objects[1]
+ col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
- col = self.robot.collision_model.geometry_objects[1]
+ col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.parent == 4)
col.parent = 5
self.assertTrue(col.parent == 5)
@@ -39,14 +39,14 @@ class TestGeometryObjectBindings(unittest.TestCase):
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
- col = self.robot.collision_model.geometry_objects[1]
+ col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
- col = self.robot.collision_model.geometry_objects[1]
+ col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.mesh_path == expected_mesh_path)
if __name__ == '__main__':
diff --git a/src/algorithm/collisions.hpp b/src/algorithm/collisions.hpp
index 5e4ba7bd3..28a6979e8 100644
--- a/src/algorithm/collisions.hpp
+++ b/src/algorithm/collisions.hpp
@@ -104,8 +104,8 @@ namespace se3
{
for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeoms; ++i)
{
- const Model::JointIndex & parent = model_geom.geometry_objects[i].parent;
- data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometry_objects[i].placement);
+ const Model::JointIndex & parent = model_geom.geometryObjects[i].parent;
+ data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryObjects[i].placement);
data_geom.oMg_fcl[i] = toFclTransform3f(data_geom.oMg[i]);
}
}
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 96ae0df8a..2e88d9f9f 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -242,7 +242,7 @@ struct GeometryObject
typedef Model::JointIndex JointIndex;
typedef Model::GeomIndex GeomIndex;
- typedef std::list<GeomIndex> GeomIndexList;
+ typedef std::vector<GeomIndex> GeomIndexList;
/// \brief A const reference to the reference model.
const se3::Model & model;
@@ -251,7 +251,7 @@ struct GeometryObject
Index ngeoms;
/// \brief Vector of GeometryObjects used for collision computations
- std::vector<GeometryObject> geometry_objects;
+ std::vector<GeometryObject> geometryObjects;
/// \brief A list of associated collision GeometryObjects to a given joint Id.
/// Inner objects can be seen as geometry objects that directly move when the associated joint moves
@@ -264,7 +264,7 @@ struct GeometryObject
GeometryModel(const se3::Model & model)
: model(model)
, ngeoms(0)
- , geometry_objects()
+ , geometryObjects()
, innerObjects()
, outerObjects()
{}
@@ -280,7 +280,7 @@ struct GeometryObject
* @param[in] geom_name The name of the Geometry Object
* @param[in] mesh_path The absolute path to the mesh
*
- * @return The index of the new added GeometryObject in geometry_objects
+ * @return The index of the new added GeometryObject in geometryObjects
*/
inline GeomIndex addGeometryObject(const JointIndex parent, const fcl::CollisionObject & co,
const SE3 & placement, const std::string & geom_name = "",
@@ -303,7 +303,7 @@ struct GeometryObject
*
* @param[in] name Name of the GeometryObject
*
- * @return True if the GeometryObject exists in the geometry_objects.
+ * @return True if the GeometryObject exists in the geometryObjects.
*/
bool existGeometryName(const std::string & name) const;
@@ -400,8 +400,8 @@ struct GeometryObject
{
const std::size_t num_max_collision_pairs = (model_geom.ngeoms * (model_geom.ngeoms-1))/2;
collision_pairs.reserve(num_max_collision_pairs);
- distance_results.resize(num_max_collision_pairs, DistanceResult( fcl::DistanceResult(), 0, 0) );
- collision_results.resize(num_max_collision_pairs, CollisionResult( fcl::CollisionResult(), 0, 0));
+ distance_results.reserve(num_max_collision_pairs);
+ collision_results.reserve(num_max_collision_pairs);
}
~GeometryData() {};
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 074aa088f..9bf64da7e 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -51,7 +51,7 @@ namespace se3
Index idx = (Index) (ngeoms ++);
- geometry_objects.push_back(GeometryObject( geom_name, parent, co,
+ geometryObjects.push_back(GeometryObject( geom_name, parent, co,
placement, mesh_path));
addInnerObject(parent, idx);
return idx;
@@ -61,27 +61,27 @@ namespace se3
inline GeometryModel::GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
- std::vector<GeometryObject>::const_iterator it = std::find_if(geometry_objects.begin(),
- geometry_objects.end(),
+ std::vector<GeometryObject>::const_iterator it = std::find_if(geometryObjects.begin(),
+ geometryObjects.end(),
boost::bind(&GeometryObject::name, _1) == name
);
- return GeometryModel::GeomIndex(it - geometry_objects.begin());
+ return GeometryModel::GeomIndex(it - geometryObjects.begin());
}
inline bool GeometryModel::existGeometryName(const std::string & name) const
{
- return std::find_if(geometry_objects.begin(),
- geometry_objects.end(),
- boost::bind(&GeometryObject::name, _1) == name) != geometry_objects.end();
+ return std::find_if(geometryObjects.begin(),
+ geometryObjects.end(),
+ boost::bind(&GeometryObject::name, _1) == name) != geometryObjects.end();
}
inline const std::string& GeometryModel::getGeometryName(const GeomIndex index) const
{
- assert( index < (GeomIndex)geometry_objects.size() );
- return geometry_objects[index].name;
+ assert( index < (GeomIndex)geometryObjects.size() );
+ return geometryObjects[index].name;
}
@@ -111,7 +111,7 @@ namespace se3
for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeoms);++i)
{
- os << model_geom.geometry_objects[i] <<std::endl;
+ os << model_geom.geometryObjects[i] <<std::endl;
}
return os;
@@ -250,8 +250,8 @@ namespace se3
fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
fcl::CollisionResult collisionResult;
- fcl::collide (model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::collide (model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
collisionRequest, collisionResult);
return CollisionResult (collisionResult, co1, co2);
@@ -288,8 +288,8 @@ namespace se3
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
- fcl::distance ( model_geom.geometry_objects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometry_objects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::distance ( model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
distanceRequest, result);
return DistanceResult (result, co1, co2);
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
index f2e13dcff..3c5698bbc 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -68,8 +68,8 @@ namespace se3
.def("getGeometryId",&GeometryModelPythonVisitor::getGeometryId)
.def("existGeometryName",&GeometryModelPythonVisitor::existGeometryName)
.def("getGeometryName",&GeometryModelPythonVisitor::getGeometryName)
- .add_property("geometry_objects",
- bp::make_function(&GeometryModelPythonVisitor::geometry_objects,
+ .add_property("geometryObjects",
+ bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
bp::return_internal_reference<>()) )
.def("__str__",&GeometryModelPythonVisitor::toString)
@@ -87,7 +87,7 @@ namespace se3
static std::string getGeometryName(const GeometryModelHandler & gmodelPtr, const GeomIndex index)
{ return gmodelPtr->getGeometryName(index);}
- static std::vector<GeometryObject> & geometry_objects( GeometryModelHandler & m ) { return m->geometry_objects; }
+ static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
diff --git a/src/python/robot_wrapper.py b/src/python/robot_wrapper.py
index d5dde02f8..1b5cf3742 100644
--- a/src/python/robot_wrapper.py
+++ b/src/python/robot_wrapper.py
@@ -170,7 +170,7 @@ class RobotWrapper(object):
self.viewer.gui.createGroup(nodeName)
# iterate over visuals and create the meshes in the viewer
- for visual in self.visual_model.geometry_objects :
+ for visual in self.visual_model.geometryObjects :
meshName = self.viewerNodeNames(visual)
meshPath = visual.mesh_path
self.viewer.gui.addMesh(meshName, meshPath)
@@ -187,7 +187,7 @@ class RobotWrapper(object):
self.updateGeometryPlacements(q)
- for visual in self.visual_model.geometry_objects :
+ for visual in self.visual_model.geometryObjects :
M = self.collision_data.oMg[self.visual_model.getGeometryId(visual.name)]
pinocchioConf = utils.se3ToXYZQUAT(M)
viewerConf = utils.XYZQUATToViewerConfiguration(pinocchioConf)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index fc5e4623b..82cc738eb 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -132,8 +132,8 @@ std::ostream& operator<<(std::ostream& os, const std::pair < se3::Model, se3::Ge
for(se3::GeometryModel::Index i=0;i<(se3::GeometryModel::Index)(robot.second.ngeoms);++i)
{
- os << "Object n " << i << " : " << robot.second.geometry_objects[i].name << ": attached to joint = " << robot.second.geometry_objects[i].parent
- << "=" << robot.first.getJointName(robot.second.geometry_objects[i].parent) << std::endl;
+ os << "Object n " << i << " : " << robot.second.geometryObjects[i].name << ": attached to joint = " << robot.second.geometryObjects[i].parent
+ << "=" << robot.first.getJointName(robot.second.geometryObjects[i].parent) << std::endl;
}
return os;
}
--
GitLab