24 , mStaticBreakRatio(pStaticBreakRatio)
25 , mTogetherRatio(pTogetherRatio)
26 , mMaxAngleDeviation(pMaxAngleDeviation)
37 double bestDistance = std::numeric_limits<double>::max();
45 double currentClosestDistance = std::numeric_limits<double>::max();
46 int currentBestBreaks = 1;
47 int currentBestCommonPositions = 1;
53 if (first == second) {
75 int commonPositions = 0;
76 double averageDistance = 0;
79 for(
unsigned int i = 0; i < first->mObjectSet->objects.size(); i++)
85 if (!firstObject || !secondObject)
98 if(commonPositions < (
double) first->mObjectSet->objects.size() *
mTogetherRatio)
104 averageDistance /= (double) commonPositions;
106 int staticBreaks = 0;
107 bool firstRun =
true;
108 Eigen::Vector3d directionVector;
111 for(
unsigned int i = 0; i < first->mObjectSet->objects.size(); i++)
117 if (!firstObject || !secondObject)
130 Eigen::Vector3d currentDirection = this->
getDirectionVector(firstObject, secondObject);
137 directionVector = currentDirection;
144 (!currentBest || (currentClosestDistance > averageDistance))
147 currentBest = second;
148 currentClosestDistance = averageDistance;
149 currentBestBreaks = staticBreaks;
150 currentBestCommonPositions = commonPositions;
155 double conf = 1 - (double) currentBestBreaks / (
double) currentBestCommonPositions;
157 || (conf > this->
score 158 || (conf == this->
score && bestDistance > currentClosestDistance)))
169 bestDistance = currentClosestDistance;
178 double bestDistance = std::numeric_limits<double>::max();
186 double currentClosestDistance = std::numeric_limits<double>::max();
187 int currentBestBreaks = 1;
188 int currentBestCommonPositions = 1;
208 int commonPositions = 0;
209 double averageDistance = 0;
212 for(
unsigned int i = 0; i < first->mObjectSet->objects.size(); i++)
218 if (!firstObject || !secondObject)
229 if(commonPositions < (
double) first->mObjectSet->objects.size() *
mTogetherRatio)
233 averageDistance /= (double) commonPositions;
235 int staticBreaks = 0;
236 bool firstRun =
true;
237 Eigen::Vector3d directionVector;
240 for(
unsigned int i = 0; i < first->mObjectSet->objects.size(); i++)
246 if (!firstObject || !secondObject)
259 Eigen::Vector3d currentDirection = this->
getDirectionVector(firstObject, secondObject);
266 directionVector = currentDirection;
273 (!currentBest || (currentClosestDistance > averageDistance))
276 currentBest = pChild;
277 currentClosestDistance = averageDistance;
278 currentBestBreaks = staticBreaks;
279 currentBestCommonPositions = commonPositions;
283 double conf = 1 - (double) currentBestBreaks / (
double) currentBestCommonPositions;
285 || (conf > this->
score 286 || (conf == this->
score && bestDistance > currentClosestDistance)))
297 bestDistance = currentClosestDistance;
310 double bestViewRatio = 0;
313 double bestMovement = 0;
343 double ratio = (double) views / (
double) candidate->mObjectSet->objects.size();
344 if (ratio > bestViewRatio || (ratio == bestViewRatio && movement < bestMovement)) {
345 bestViewRatio = ratio;
346 bestMovement = movement;
353 if(candidate != root)
354 root->addChild(candidate);
369 Eigen::Vector3d firstToSecond = second->pose->point->eigen - first->pose->point->eigen;
372 Eigen::Quaternion<double> firstRotation = first->pose->quat->eigen;
375 return firstRotation.inverse()._transformVector(firstToSecond).normalized();
boost::shared_ptr< TreeNode > getBestParentNode()
~DirectionRelationHeuristic()
DirectionRelationHeuristic(const double pStaticBreakRatio, const double pTogetherRatio, const double pMaxAngleDeviation)
static double getDistanceBetweenPoints(const Eigen::Vector3d p1, const Eigen::Vector3d p2)
Eigen::Vector3d getDirectionVector(const boost::shared_ptr< ISM::Object > first, const boost::shared_ptr< ISM::Object > second)
double mMaxAngleDeviation
void apply(std::vector< boost::shared_ptr< TreeNode > > pNodes)
static double rad2deg(double rad)
boost::shared_ptr< TreeNode > getBestCluster()
std::vector< boost::shared_ptr< TreeNode > > candidates