Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/sampling/sample-db.cc	Lines:	92	253	36.4 %
Date:	2024-02-02 12:21:48	Branches:	69	384	18.0 %


#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/shape/geometric_shapes.h>

#include <fstream>
#include <hpp/pinocchio/collision-object.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/rbprm/sampling/sample-db.hh>
#include <hpp/rbprm/tools.hh>
#include <iostream>
#include <string>

using namespace hpp;
using namespace hpp::pinocchio;
using namespace hpp::rbprm;
using namespace hpp::rbprm::sampling;
using namespace fcl;

namespace {
octomap::OcTree* generateOctree(const T_Sample& samples,
                                const double resolution) {
  octomap::OcTree* octTree = new octomap::OcTree(resolution);
  for (SampleVector_t::const_iterator cit = samples.begin();
       cit != samples.end(); ++cit) {
    const fcl::Vec3f& position = cit->effectorPosition_;
    octomap::point3d endpoint((float)position[0], (float)position[1],
                              (float)position[2]);
    octTree->updateNode(endpoint, true);
  }
  octTree->updateInnerOccupancy();
  return octTree;
}

typedef std::map<long int, std::vector<std::size_t> > T_VoxelSample;
T_VoxelSample getSamplesPerVoxel(
    const fcl::shared_ptr<const octomap::OcTree>& octTree,
    const SampleVector_t& samples) {
  T_VoxelSample res;
  std::size_t sid = 0;
  for (SampleVector_t::const_iterator cit = samples.begin();
       cit != samples.end(); ++cit, ++sid) {
    const fcl::Vec3f& position = cit->effectorPosition_;
    long int id = octTree->search(position[0], position[1], position[2]) -
                  octTree->getRoot();
    T_VoxelSample::iterator it = res.find(id);
    if (it != res.end())
      it->second.push_back(sid);
    else {
      std::vector<std::size_t> samples;
      samples.push_back(sid);
      res.insert(std::make_pair(id, samples));
    }
  }
  return res;
}

void alignSampleOrderWithOctree(SampleDB& db) {
  std::vector<std::size_t>
      realignOrderIds;  // indicate how to realign each value in value vector
  // assumes samples are sorted, so they are already sorted by interest
  // within octree
  T_Sample reorderedSamples;
  reorderedSamples.reserve(db.samples_.size());
  T_VoxelSample samplesPerVoxel =
      getSamplesPerVoxel(db.octomapTree_, db.samples_);

  // now sort all voxels by id for continuity
  // so that samples in adjacent voxels are also aligned
  std::vector<long int> voxelIds;
  for (T_VoxelSample::const_iterator vit = samplesPerVoxel.begin();
       vit != samplesPerVoxel.end(); ++vit) {
    voxelIds.push_back(vit->first);
  }
  std::sort(voxelIds.begin(), voxelIds.end());

  T_VoxelSampleId reorderedSamplesPerVoxel;
  std::size_t currentNewIndex = 0;
  for (std::vector<long int>::const_iterator vit = voxelIds.begin();
       vit != voxelIds.end(); ++vit) {
    const long int& voxelId = *vit;
    const std::vector<std::size_t>& sampleIds = samplesPerVoxel[voxelId];
    std::size_t startSampleId = currentNewIndex;
    for (std::vector<std::size_t>::const_iterator sit = sampleIds.begin();
         sit != sampleIds.end(); ++sit) {
      reorderedSamples.push_back(db.samples_[*sit]);
      reorderedSamples.back().id_ = currentNewIndex;
      realignOrderIds.push_back(*sit);
      ++currentNewIndex;
    }
    reorderedSamplesPerVoxel.insert(std::make_pair(
        voxelId,
        std::make_pair(startSampleId, currentNewIndex - startSampleId)));
  }
  // Now reorder all values
  T_Values reorderedValues;
  for (T_Values::const_iterator cit = db.values_.begin();
       cit != db.values_.end(); ++cit) {
    T_Double vals;
    vals.reserve(realignOrderIds.size());
    for (std::vector<std::size_t>::const_iterator rit = realignOrderIds.begin();
         rit != realignOrderIds.end(); ++rit) {
      vals.push_back(cit->second[*rit]);
    }
    reorderedValues.insert(std::make_pair(cit->first, vals));
  }
  db.samplesInVoxels_ = reorderedSamplesPerVoxel;
  db.values_ = reorderedValues;
  db.samples_ = reorderedSamples;
}

void sortDB(SampleDB& database) {
  sample_greater sort;
  std::sort(database.samples_.begin(), database.samples_.end(), sort);
  alignSampleOrderWithOctree(database);
}

std::map<std::size_t, fcl::CollisionObject*> generateBoxesFromOctomap(
    const fcl::shared_ptr<const octomap::OcTree>& octTree,
    const fcl::OcTree* tree) {
  std::map<std::size_t, fcl::CollisionObject*> boxes;
  std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree->toBoxes();
  // boxes.reserve(boxes_.size());
  for (std::size_t i = 0; i < boxes_.size(); ++i) {
    FCL_REAL x = boxes_[i][0];
    FCL_REAL y = boxes_[i][1];
    FCL_REAL z = boxes_[i][2];
    FCL_REAL size = boxes_[i][3];
    FCL_REAL cost = boxes_[i][4];
    FCL_REAL threshold = boxes_[i][5];
    std::size_t id = octTree->search(x, y, z) - octTree->getRoot();

    Box* box = new Box(size, size, size);
    box->cost_density = cost;
    box->threshold_occupied = threshold;
    fcl::CollisionObject* obj = new fcl::CollisionObject(
        fcl::CollisionGeometryPtr_t(box), fcl::Transform3f(Vec3f(x, y, z)));
    boxes.insert(std::make_pair(id, obj));
  }
  return boxes;
}
}  // namespace

SampleDB::SampleDB(const pinocchio::JointPtr_t limb,
                   const std::string& effector, const std::size_t nbSamples,
                   const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset,
                   const double resolution, const T_evaluate& data,
                   const std::string& staticValue)
    : resolution_(resolution),
      samples_(GenerateSamples(limb, effector, nbSamples, offset, limbOffset)),
      octomapTree_(generateOctree(samples_, resolution)),
      octree_(new fcl::OcTree(octomapTree_)),
      geometry_(fcl::CollisionGeometryPtr_t(octree_)),
      treeObject_(geometry_),
      boxes_(generateBoxesFromOctomap(octomapTree_, octree_)) {
  for (T_evaluate::const_iterator cit = data.begin(); cit != data.end();
       ++cit) {
    bool sort = staticValue == cit->first;
    addValue(*this, cit->first, cit->second, sort, false);
  }
  sortDB(*this);
}

SampleDB::~SampleDB() {
  for (std::map<std::size_t, fcl::CollisionObject*>::const_iterator it =
           boxes_.begin();
       it != boxes_.end(); ++it) {
    delete it->second;
  }
}

OctreeReport::OctreeReport(const Sample* s, const fcl::Contact c,
                           const double v, const fcl::Vec3f& normal,
                           const fcl::Vec3f& v1, const fcl::Vec3f& v2,
                           const fcl::Vec3f& v3)
    : sample_(s),
      contact_(c),
      value_(v),
      normal_(normal),
      v1_(v1),
      v2_(v2),
      v3_(v3) {
  // NOTHING
}

SampleDB& hpp::rbprm::sampling::addValue(SampleDB& database,
                                         const std::string& valueName,
                                         const evaluate eval,
                                         bool isStaticValue, bool sortSamples) {
  T_Values::const_iterator cit = database.values_.find(valueName);
  if (cit != database.values_.end()) {
    hppDout(warning, "value already existing for database " << valueName);
    return database;
  } else {
    double maxValue = -std::numeric_limits<double>::max();
    double minValue = std::numeric_limits<double>::max();
    T_Double values;
    values.reserve(database.samples_.size());
    for (T_Sample::iterator it = database.samples_.begin();
         it != database.samples_.end(); ++it) {
      double val = eval(database, *it);
      maxValue = std::max(maxValue, val);
      minValue = std::min(minValue, val);
      values.push_back(val);
    }
    database.valueBounds_.insert(
        std::make_pair(valueName, std::make_pair(minValue, maxValue)));
    // now normalize values
    double max_min = maxValue - minValue;
    for (T_Double::iterator it = values.begin(); it != values.end(); ++it) {
      *it = (max_min != 0) ? (*it - minValue) / max_min : 0;
    }
    if (isStaticValue) {
      for (size_t id = 0; id < database.samples_.size(); id++)
        database.samples_[id].staticValue_ = values[id];
    }
    database.values_.insert(std::make_pair(valueName, values));
    if (sortSamples) sortDB(database);
  }
  return database;
}

// TODO Samples should be Vec3f
bool rbprm::sampling::GetCandidates(
    const SampleDB& sc, const fcl::Transform3f& treeTrf,
    const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
    hpp::rbprm::sampling::T_OctreeReport& reports, const HeuristicParam& params,
    const heuristic evaluate) {
  fcl::CollisionRequest req(fcl::CONTACT, 1000);
  fcl::CollisionResult cResult;
  fcl::CollisionObject* obj = o2->fcl();
  fcl::collide(sc.geometry_.get(), treeTrf, obj->collisionGeometry().get(),
               obj->getTransform(), req, cResult);
  sampling::T_VoxelSampleId::const_iterator voxelIt;
  Eigen::Vector3d eDir(direction[0], direction[1], direction[2]);
  eDir.normalize();
  std::vector<long int> visited;
  int totalSamples = 0;
  int okay = 0;
  for (std::size_t index = 0; index < cResult.numContacts(); ++index) {
    const Contact& contact = cResult.getContact(index);
    if (std::find(visited.begin(), visited.end(), contact.b1) == visited.end())
      visited.push_back(contact.b1);
    // verifying that position is theoritically reachable from next position
    {
      voxelIt = sc.samplesInVoxels_.find(contact.b1);
      if (voxelIt != sc.samplesInVoxels_.end()) {
        const VoxelSampleId& voxelSampleIds = voxelIt->second;
        totalSamples += (int)voxelSampleIds.second;
        for (T_Sample::const_iterator sit =
                 sc.samples_.begin() + voxelSampleIds.first;
             sit !=
             sc.samples_.begin() + voxelSampleIds.first + voxelSampleIds.second;
             ++sit) {
          // find normal id
          assert(contact.o2->getObjectType() ==
                 fcl::OT_BVH);  // only works with meshes
          const fcl::BVHModel<fcl::OBBRSS>* surface =
              static_cast<const fcl::BVHModel<fcl::OBBRSS>*>(contact.o2);
          fcl::Vec3f normal;
          const fcl::Triangle& tr = surface->tri_indices[contact.b2];
          const fcl::Vec3f& v1 = surface->vertices[tr[0]];
          const fcl::Vec3f& v2 = surface->vertices[tr[1]];
          const fcl::Vec3f& v3 = surface->vertices[tr[2]];
          normal = (v2 - v1).cross(v3 - v1);
          normal.normalize();
          Eigen::Vector3d eNormal(normal[0], normal[1], normal[2]);
          OctreeReport report(
              &(*sit), contact,
              evaluate ? ((*evaluate)(*sit, eDir, eNormal, params)) : 0,
              eNormal, v1, v2, v3);
          ++okay;
          reports.insert(report);
        }
      } else
        hppDout(warning, "no voxels in specified triangle : " << contact.b1);
    }
  }
  return !reports.empty();
}

rbprm::sampling::T_OctreeReport rbprm::sampling::GetCandidates(
    const SampleDB& sc, const fcl::Transform3f& treeTrf,
    const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
    const HeuristicParam& params, const heuristic evaluate) {
  rbprm::sampling::T_OctreeReport report;
  GetCandidates(sc, treeTrf, o2, direction, report, params, evaluate);
  return report;
}

using std::ostringstream;
using namespace tools::io;

void writeSample(const Sample& sample, std::ostream& output) {
  output << "sample" << std::endl;
  output << sample.id_ << std::endl;
  output << sample.length_ << std::endl;
  output << sample.startRank_ << std::endl;
  output << sample.staticValue_ << std::endl;
  writeVecFCL(sample.effectorPosition_, output);
  output << std::endl;
  writeVecFCL(sample.effectorPositionInLimbFrame_, output);
  output << std::endl;
  writeMatrix(sample.configuration_, output);
  output << std::endl;
  writeMatrix(sample.jacobian_, output);
  output << std::endl;
  writeMatrix(sample.jacobianProduct_, output);
  output << std::endl;
}

void writeSamples(const SampleDB& database, std::ostream& output) {
  for (T_Sample::const_iterator cit = database.samples_.begin();
       cit != database.samples_.end(); ++cit) {
    writeSample(*cit, output);
  }
}

void writeValues(const SampleDB& database, std::ostream& output) {
  for (T_Values::const_iterator cit = database.values_.begin();
       cit != database.values_.end(); ++cit) {
    output << "value" << std::endl << cit->first << std::endl;
    for (T_Double::const_iterator dit = cit->second.begin();
         dit != cit->second.end(); ++dit) {
      output << *dit << std::endl;
    }
    output << std::endl;
  }
}

Sample readSample(std::ifstream& myfile, std::string& line) {
  std::size_t id, length, startRank;
  double staticValue;
  fcl::Vec3f effpos, effPosInLimbFrame;
  Eigen::VectorXd conf;
  Eigen::MatrixXd jav, ajcprod;
  id = StrToI(myfile);
  length = StrToI(myfile);
  startRank = StrToI(myfile);
  staticValue = StrToD(myfile);
  effpos = readVecFCL(myfile, line);
  effPosInLimbFrame = readVecFCL(myfile, line);
  conf = readMatrix(myfile, line);
  jav = readMatrix(myfile, line);
  ajcprod = readMatrix(myfile, line);
  return Sample(id, length, startRank, staticValue, effpos, effPosInLimbFrame,
                conf, jav, ajcprod);
}

void readValue(T_Values& values, const std::size_t& size, std::ifstream& myfile,
               std::string& line) {
  getline(myfile, line);  // name
  std::string valueName = line;
  T_Double vals;
  vals.reserve(size);
  for (std::size_t i = 0; i < size; ++i) {
    getline(myfile, line);
    vals.push_back(StrToD(line));
  }
  values.insert(std::make_pair(valueName, vals));
}

bool hpp::rbprm::sampling::saveLimbDatabase(const SampleDB& database,
                                            std::ofstream& fp) {
  fp << database.samples_.size() << std::endl;
  fp << database.resolution_ << std::endl;
  writeSamples(database, fp);
  writeValues(database, fp);
  return true;
}

SampleDB::SampleDB(std::ifstream& myfile, bool loadValues)
    : treeObject_(CollisionGeometryPtr_t(new fcl::Box(1, 1, 1))) {
  if (!myfile.good()) throw std::runtime_error("Impossible to open database");
  if (myfile.is_open()) {
    std::string line;
    getline(myfile, line);
    std::size_t size = StrToI(line);
    samples_.reserve(size);
    getline(myfile, line);
    resolution_ = StrToD(line);
    while (myfile.good()) {
      getline(myfile, line);
      if (line.find("sample") != std::string::npos)
        samples_.push_back(readSample(myfile, line));
      else if (line.find("value") != std::string::npos && loadValues)
        readValue(values_, size, myfile, line);
    }
  }
  octomapTree_ = fcl::shared_ptr<const octomap::OcTree>(
      generateOctree(samples_, resolution_));
  octree_ = new fcl::OcTree(octomapTree_);
  geometry_ = fcl::CollisionGeometryPtr_t(octree_);
  treeObject_ = fcl::CollisionObject(geometry_);
  boxes_ = generateBoxesFromOctomap(octomapTree_, octree_);
  alignSampleOrderWithOctree(*this);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include <hpp/fcl/BVH/BVH_model.h>
2			#include <hpp/fcl/collision.h>
3			#include <hpp/fcl/shape/geometric_shapes.h>
4
5			#include <fstream>
6			#include <hpp/pinocchio/collision-object.hh>
7			#include <hpp/pinocchio/joint.hh>
8			#include <hpp/rbprm/sampling/sample-db.hh>
9			#include <hpp/rbprm/tools.hh>
10			#include <iostream>
11			#include <string>
12
13			using namespace hpp;
14			using namespace hpp::pinocchio;
15			using namespace hpp::rbprm;
16			using namespace hpp::rbprm::sampling;
17			using namespace fcl;
18
19			namespace {
20		10	octomap::OcTree* generateOctree(const T_Sample& samples,
21			const double resolution) {
22	✓✗	10	octomap::OcTree* octTree = new octomap::OcTree(resolution);
23		10010	for (SampleVector_t::const_iterator cit = samples.begin();
24	✓✓	20010	cit != samples.end(); ++cit) {
25		10000	const fcl::Vec3f& position = cit->effectorPosition_;
26		30000	octomap::point3d endpoint((float)position[0], (float)position[1],
27	✓✗✓✗ ✓✗	10000	(float)position[2]);
28	✓✗	10000	octTree->updateNode(endpoint, true);
29			}
30		10	octTree->updateInnerOccupancy();
31		10	return octTree;
32			}
33
34			typedef std::map<long int, std::vector<std::size_t> > T_VoxelSample;
35		10	T_VoxelSample getSamplesPerVoxel(
36			const fcl::shared_ptr<const octomap::OcTree>& octTree,
37			const SampleVector_t& samples) {
38		10	T_VoxelSample res;
39		10	std::size_t sid = 0;
40		10010	for (SampleVector_t::const_iterator cit = samples.begin();
41	✓✓	20010	cit != samples.end(); ++cit, ++sid) {
42		10000	const fcl::Vec3f& position = cit->effectorPosition_;
43	✓✗✓✗ ✓✗✓✗	10000	long int id = octTree->search(position[0], position[1], position[2]) -
44		10000	octTree->getRoot();
45	✓✗	10000	T_VoxelSample::iterator it = res.find(id);
46	✓✓	10000	if (it != res.end())
47	✓✗	2549	it->second.push_back(sid);
48			else {
49		7451	std::vector<std::size_t> samples;
50	✓✗	7451	samples.push_back(sid);
51	✓✗✓✗	7451	res.insert(std::make_pair(id, samples));
52			}
53			}
54		20	return res;
55			}
56
57		10	void alignSampleOrderWithOctree(SampleDB& db) {
58			std::vector<std::size_t>
59		20	realignOrderIds; // indicate how to realign each value in value vector
60			// assumes samples are sorted, so they are already sorted by interest
61			// within octree
62	✓✗	20	T_Sample reorderedSamples;
63	✓✗	10	reorderedSamples.reserve(db.samples_.size());
64			T_VoxelSample samplesPerVoxel =
65	✓✗	20	getSamplesPerVoxel(db.octomapTree_, db.samples_);
66
67			// now sort all voxels by id for continuity
68			// so that samples in adjacent voxels are also aligned
69		20	std::vector<long int> voxelIds;
70		7461	for (T_VoxelSample::const_iterator vit = samplesPerVoxel.begin();
71	✓✓	14912	vit != samplesPerVoxel.end(); ++vit) {
72	✓✗	7451	voxelIds.push_back(vit->first);
73			}
74	✓✗	10	std::sort(voxelIds.begin(), voxelIds.end());
75
76		20	T_VoxelSampleId reorderedSamplesPerVoxel;
77		10	std::size_t currentNewIndex = 0;
78		7461	for (std::vector<long int>::const_iterator vit = voxelIds.begin();
79	✓✓	7461	vit != voxelIds.end(); ++vit) {
80		7451	const long int& voxelId = *vit;
81	✓✗	7451	const std::vector<std::size_t>& sampleIds = samplesPerVoxel[voxelId];
82		7451	std::size_t startSampleId = currentNewIndex;
83		17451	for (std::vector<std::size_t>::const_iterator sit = sampleIds.begin();
84	✓✓	17451	sit != sampleIds.end(); ++sit) {
85	✓✗	10000	reorderedSamples.push_back(db.samples_[*sit]);
86		10000	reorderedSamples.back().id_ = currentNewIndex;
87	✓✗	10000	realignOrderIds.push_back(*sit);
88		10000	++currentNewIndex;
89			}
90			reorderedSamplesPerVoxel.insert(std::make_pair(
91			voxelId,
92	✓✗✓✗ ✓✗	7451	std::make_pair(startSampleId, currentNewIndex - startSampleId)));
93			}
94			// Now reorder all values
95		20	T_Values reorderedValues;
96		10	for (T_Values::const_iterator cit = db.values_.begin();
97	✗✓	10	cit != db.values_.end(); ++cit) {
98			T_Double vals;
99			vals.reserve(realignOrderIds.size());
100			for (std::vector<std::size_t>::const_iterator rit = realignOrderIds.begin();
101			rit != realignOrderIds.end(); ++rit) {
102			vals.push_back(cit->second[*rit]);
103			}
104			reorderedValues.insert(std::make_pair(cit->first, vals));
105			}
106	✓✗	10	db.samplesInVoxels_ = reorderedSamplesPerVoxel;
107	✓✗	10	db.values_ = reorderedValues;
108	✓✗	10	db.samples_ = reorderedSamples;
109		10	}
110
111		10	void sortDB(SampleDB& database) {
112			sample_greater sort;
113	✓✗	10	std::sort(database.samples_.begin(), database.samples_.end(), sort);
114	✓✗	10	alignSampleOrderWithOctree(database);
115		10	}
116
117		10	std::map<std::size_t, fcl::CollisionObject*> generateBoxesFromOctomap(
118			const fcl::shared_ptr<const octomap::OcTree>& octTree,
119			const fcl::OcTree* tree) {
120		10	std::map<std::size_t, fcl::CollisionObject*> boxes;
121	✓✗	20	std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree->toBoxes();
122			// boxes.reserve(boxes_.size());
123	✓✓	7461	for (std::size_t i = 0; i < boxes_.size(); ++i) {
124		7451	FCL_REAL x = boxes_[i][0];
125		7451	FCL_REAL y = boxes_[i][1];
126		7451	FCL_REAL z = boxes_[i][2];
127		7451	FCL_REAL size = boxes_[i][3];
128		7451	FCL_REAL cost = boxes_[i][4];
129		7451	FCL_REAL threshold = boxes_[i][5];
130	✓✗	7451	std::size_t id = octTree->search(x, y, z) - octTree->getRoot();
131
132	✓✗✓✗	7451	Box* box = new Box(size, size, size);
133		7451	box->cost_density = cost;
134		7451	box->threshold_occupied = threshold;
135			fcl::CollisionObject* obj = new fcl::CollisionObject(
136	✓✗✓✗ ✓✗✓✗ ✓✗	7451	fcl::CollisionGeometryPtr_t(box), fcl::Transform3f(Vec3f(x, y, z)));
137	✓✗✓✗	7451	boxes.insert(std::make_pair(id, obj));
138			}
139		20	return boxes;
140			}
141			} // namespace
142
143		10	SampleDB::SampleDB(const pinocchio::JointPtr_t limb,
144			const std::string& effector, const std::size_t nbSamples,
145			const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset,
146			const double resolution, const T_evaluate& data,
147		10	const std::string& staticValue)
148			: resolution_(resolution),
149			samples_(GenerateSamples(limb, effector, nbSamples, offset, limbOffset)),
150		10	octomapTree_(generateOctree(samples_, resolution)),
151	✓✗✓✗	10	octree_(new fcl::OcTree(octomapTree_)),
152			geometry_(fcl::CollisionGeometryPtr_t(octree_)),
153		10	treeObject_(geometry_),
154	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	boxes_(generateBoxesFromOctomap(octomapTree_, octree_)) {
155	✗✓	10	for (T_evaluate::const_iterator cit = data.begin(); cit != data.end();
156			++cit) {
157			bool sort = staticValue == cit->first;
158			addValue(*this, cit->first, cit->second, sort, false);
159			}
160	✓✗	10	sortDB(*this);
161		10	}
162
163		10	SampleDB::~SampleDB() {
164		7451	for (std::map<std::size_t, fcl::CollisionObject*>::const_iterator it =
165		10	boxes_.begin();
166	✓✓	14912	it != boxes_.end(); ++it) {
167	✓✗	7451	delete it->second;
168			}
169		10	}
170
171			OctreeReport::OctreeReport(const Sample* s, const fcl::Contact c,
172			const double v, const fcl::Vec3f& normal,
173			const fcl::Vec3f& v1, const fcl::Vec3f& v2,
174			const fcl::Vec3f& v3)
175			: sample_(s),
176			contact_(c),
177			value_(v),
178			normal_(normal),
179			v1_(v1),
180			v2_(v2),
181			v3_(v3) {
182			// NOTHING
183			}
184
185			SampleDB& hpp::rbprm::sampling::addValue(SampleDB& database,
186			const std::string& valueName,
187			const evaluate eval,
188			bool isStaticValue, bool sortSamples) {
189			T_Values::const_iterator cit = database.values_.find(valueName);
190			if (cit != database.values_.end()) {
191			hppDout(warning, "value already existing for database " << valueName);
192			return database;
193			} else {
194			double maxValue = -std::numeric_limits<double>::max();
195			double minValue = std::numeric_limits<double>::max();
196			T_Double values;
197			values.reserve(database.samples_.size());
198			for (T_Sample::iterator it = database.samples_.begin();
199			it != database.samples_.end(); ++it) {
200			double val = eval(database, *it);
201			maxValue = std::max(maxValue, val);
202			minValue = std::min(minValue, val);
203			values.push_back(val);
204			}
205			database.valueBounds_.insert(
206			std::make_pair(valueName, std::make_pair(minValue, maxValue)));
207			// now normalize values
208			double max_min = maxValue - minValue;
209			for (T_Double::iterator it = values.begin(); it != values.end(); ++it) {
210			it = (max_min != 0) ? (it - minValue) / max_min : 0;
211			}
212			if (isStaticValue) {
213			for (size_t id = 0; id < database.samples_.size(); id++)
214			database.samples_[id].staticValue_ = values[id];
215			}
216			database.values_.insert(std::make_pair(valueName, values));
217			if (sortSamples) sortDB(database);
218			}
219			return database;
220			}
221
222			// TODO Samples should be Vec3f
223			bool rbprm::sampling::GetCandidates(
224			const SampleDB& sc, const fcl::Transform3f& treeTrf,
225			const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
226			hpp::rbprm::sampling::T_OctreeReport& reports, const HeuristicParam& params,
227			const heuristic evaluate) {
228			fcl::CollisionRequest req(fcl::CONTACT, 1000);
229			fcl::CollisionResult cResult;
230			fcl::CollisionObject* obj = o2->fcl();
231			fcl::collide(sc.geometry_.get(), treeTrf, obj->collisionGeometry().get(),
232			obj->getTransform(), req, cResult);
233			sampling::T_VoxelSampleId::const_iterator voxelIt;
234			Eigen::Vector3d eDir(direction[0], direction[1], direction[2]);
235			eDir.normalize();
236			std::vector<long int> visited;
237			int totalSamples = 0;
238			int okay = 0;
239			for (std::size_t index = 0; index < cResult.numContacts(); ++index) {
240			const Contact& contact = cResult.getContact(index);
241			if (std::find(visited.begin(), visited.end(), contact.b1) == visited.end())
242			visited.push_back(contact.b1);
243			// verifying that position is theoritically reachable from next position
244			{
245			voxelIt = sc.samplesInVoxels_.find(contact.b1);
246			if (voxelIt != sc.samplesInVoxels_.end()) {
247			const VoxelSampleId& voxelSampleIds = voxelIt->second;
248			totalSamples += (int)voxelSampleIds.second;
249			for (T_Sample::const_iterator sit =
250			sc.samples_.begin() + voxelSampleIds.first;
251			sit !=
252			sc.samples_.begin() + voxelSampleIds.first + voxelSampleIds.second;
253			++sit) {
254			// find normal id
255			assert(contact.o2->getObjectType() ==
256			fcl::OT_BVH); // only works with meshes
257			const fcl::BVHModel<fcl::OBBRSS>* surface =
258			static_cast<const fcl::BVHModel<fcl::OBBRSS>*>(contact.o2);
259			fcl::Vec3f normal;
260			const fcl::Triangle& tr = surface->tri_indices[contact.b2];
261			const fcl::Vec3f& v1 = surface->vertices[tr[0]];
262			const fcl::Vec3f& v2 = surface->vertices[tr[1]];
263			const fcl::Vec3f& v3 = surface->vertices[tr[2]];
264			normal = (v2 - v1).cross(v3 - v1);
265			normal.normalize();
266			Eigen::Vector3d eNormal(normal[0], normal[1], normal[2]);
267			OctreeReport report(
268			&(*sit), contact,
269			evaluate ? ((evaluate)(sit, eDir, eNormal, params)) : 0,
270			eNormal, v1, v2, v3);
271			++okay;
272			reports.insert(report);
273			}
274			} else
275			hppDout(warning, "no voxels in specified triangle : " << contact.b1);
276			}
277			}
278			return !reports.empty();
279			}
280
281			rbprm::sampling::T_OctreeReport rbprm::sampling::GetCandidates(
282			const SampleDB& sc, const fcl::Transform3f& treeTrf,
283			const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
284			const HeuristicParam& params, const heuristic evaluate) {
285			rbprm::sampling::T_OctreeReport report;
286			GetCandidates(sc, treeTrf, o2, direction, report, params, evaluate);
287			return report;
288			}
289
290			using std::ostringstream;
291			using namespace tools::io;
292
293			void writeSample(const Sample& sample, std::ostream& output) {
294			output << "sample" << std::endl;
295			output << sample.id_ << std::endl;
296			output << sample.length_ << std::endl;
297			output << sample.startRank_ << std::endl;
298			output << sample.staticValue_ << std::endl;
299			writeVecFCL(sample.effectorPosition_, output);
300			output << std::endl;
301			writeVecFCL(sample.effectorPositionInLimbFrame_, output);
302			output << std::endl;
303			writeMatrix(sample.configuration_, output);
304			output << std::endl;
305			writeMatrix(sample.jacobian_, output);
306			output << std::endl;
307			writeMatrix(sample.jacobianProduct_, output);
308			output << std::endl;
309			}
310
311			void writeSamples(const SampleDB& database, std::ostream& output) {
312			for (T_Sample::const_iterator cit = database.samples_.begin();
313			cit != database.samples_.end(); ++cit) {
314			writeSample(*cit, output);
315			}
316			}
317
318			void writeValues(const SampleDB& database, std::ostream& output) {
319			for (T_Values::const_iterator cit = database.values_.begin();
320			cit != database.values_.end(); ++cit) {
321			output << "value" << std::endl << cit->first << std::endl;
322			for (T_Double::const_iterator dit = cit->second.begin();
323			dit != cit->second.end(); ++dit) {
324			output << *dit << std::endl;
325			}
326			output << std::endl;
327			}
328			}
329
330			Sample readSample(std::ifstream& myfile, std::string& line) {
331			std::size_t id, length, startRank;
332			double staticValue;
333			fcl::Vec3f effpos, effPosInLimbFrame;
334			Eigen::VectorXd conf;
335			Eigen::MatrixXd jav, ajcprod;
336			id = StrToI(myfile);
337			length = StrToI(myfile);
338			startRank = StrToI(myfile);
339			staticValue = StrToD(myfile);
340			effpos = readVecFCL(myfile, line);
341			effPosInLimbFrame = readVecFCL(myfile, line);
342			conf = readMatrix(myfile, line);
343			jav = readMatrix(myfile, line);
344			ajcprod = readMatrix(myfile, line);
345			return Sample(id, length, startRank, staticValue, effpos, effPosInLimbFrame,
346			conf, jav, ajcprod);
347			}
348
349			void readValue(T_Values& values, const std::size_t& size, std::ifstream& myfile,
350			std::string& line) {
351			getline(myfile, line); // name
352			std::string valueName = line;
353			T_Double vals;
354			vals.reserve(size);
355			for (std::size_t i = 0; i < size; ++i) {
356			getline(myfile, line);
357			vals.push_back(StrToD(line));
358			}
359			values.insert(std::make_pair(valueName, vals));
360			}
361
362			bool hpp::rbprm::sampling::saveLimbDatabase(const SampleDB& database,
363			std::ofstream& fp) {
364			fp << database.samples_.size() << std::endl;
365			fp << database.resolution_ << std::endl;
366			writeSamples(database, fp);
367			writeValues(database, fp);
368			return true;
369			}
370
371			SampleDB::SampleDB(std::ifstream& myfile, bool loadValues)
372			: treeObject_(CollisionGeometryPtr_t(new fcl::Box(1, 1, 1))) {
373			if (!myfile.good()) throw std::runtime_error("Impossible to open database");
374			if (myfile.is_open()) {
375			std::string line;
376			getline(myfile, line);
377			std::size_t size = StrToI(line);
378			samples_.reserve(size);
379			getline(myfile, line);
380			resolution_ = StrToD(line);
381			while (myfile.good()) {
382			getline(myfile, line);
383			if (line.find("sample") != std::string::npos)
384			samples_.push_back(readSample(myfile, line));
385			else if (line.find("value") != std::string::npos && loadValues)
386			readValue(values_, size, myfile, line);
387			}
388			}
389			octomapTree_ = fcl::shared_ptr<const octomap::OcTree>(
390			generateOctree(samples_, resolution_));
391			octree_ = new fcl::OcTree(octomapTree_);
392			geometry_ = fcl::CollisionGeometryPtr_t(octree_);
393			treeObject_ = fcl::CollisionObject(geometry_);
394			boxes_ = generateBoxesFromOctomap(octomapTree_, octree_);
395			alignSampleOrderWithOctree(*this);
396			}