Skip to content

Commit

Permalink
Merge pull request #4 from fdarricau/master
Browse files Browse the repository at this point in the history 
Fix the default objective function size
  • Loading branch information
fdarricau committed May 7, 2015
2 parents d91321d + 09bb4b8 commit bbf7125
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 86 deletions.
173 changes: 90 additions & 83 deletions include/roboptim/core/manifold-map/decorator/function-on-manifold.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ namespace roboptim

// --METHOD START-- //

bool onTangentSpace = false;
bool onTangentSpace = false;
// this->fct_ = descWrap.fct();

// TODO: should be memoized for performance, although we can compute
Expand All @@ -158,69 +158,69 @@ namespace roboptim
// This lambda returns a std::pair of long representing a restriction
// of the manifold. The pair contains the starting index as the first
// element and the size of the restriction as the second element.
std::function<std::pair<long, long>(const pgs::Manifold&, const int, int)> getRestriction =
[&restrictedManifolds, &restrictions, &onTangentSpace]
(const pgs::Manifold& manifold, const int searchedIndex, int currIndex)
{
// A (-1, -1) is equivalent to no restrictions at all
std::pair<long, long> ans = std::make_pair(-1l, -1l);

for (size_t i = 0; i < restrictedManifolds.size(); ++i)
{
if (manifold.getInstanceId() == restrictedManifolds[i]->getInstanceId())
{
if (searchedIndex == currIndex)
{
ans = restrictions[(restrictions.size() == 1?0:i)];
break;
}
else
{
++currIndex;
}
}
}

// If we could not find a restriction for this manifold
// we set the restriction to the entire manifold's size
ans.first = std::max(0l, ans.first);
if (ans.second < 0)
{
ans.second = (onTangentSpace?manifold.tangentDim():manifold.representationDim());
}

return ans;
};

// These helpers function will help us to track the id of the manifolds
// we encounter while traversing the manifold trees, reusing the maps
// we first used to check the occurences counts of manifolds
std::function<int(const pgs::Manifold&)> getCurrentOccurenceCount =
[&traversalOccurences]
(const pgs::Manifold& manifold)
{
if (traversalOccurences.find(manifold.getInstanceId()) != traversalOccurences.end())
{
return traversalOccurences[manifold.getInstanceId()];
}

return 0;
};
std::function<void(const pgs::Manifold&)> incrementOccurenceCount =
[&traversalOccurences]
(const pgs::Manifold& manifold)
{
if (traversalOccurences.find(manifold.getInstanceId()) != traversalOccurences.end())
{
traversalOccurences[manifold.getInstanceId()] += 1;
}
else
{
traversalOccurences[manifold.getInstanceId()] = 1;
}
};

std::vector<const pgs::Manifold*> planarManifold;
std::function<std::pair<long, long>(const pgs::Manifold&, const int, int)> getRestriction =
[&restrictedManifolds, &restrictions, &onTangentSpace]
(const pgs::Manifold& manifold, const int searchedIndex, int currIndex)
{
// A (-1, -1) is equivalent to no restrictions at all
std::pair<long, long> ans = std::make_pair(-1l, -1l);

for (size_t i = 0; i < restrictedManifolds.size(); ++i)
{
if (manifold.getInstanceId() == restrictedManifolds[i]->getInstanceId())
{
if (searchedIndex == currIndex)
{
ans = restrictions[(restrictions.size() == 1?0:i)];
break;
}
else
{
++currIndex;
}
}
}

// If we could not find a restriction for this manifold
// we set the restriction to the entire manifold's size
ans.first = std::max(0l, ans.first);
if (ans.second < 0)
{
ans.second = (onTangentSpace?manifold.tangentDim():manifold.representationDim());
}

return ans;
};

// These helpers function will help us to track the id of the manifolds
// we encounter while traversing the manifold trees, reusing the maps
// we first used to check the occurences counts of manifolds
std::function<int(const pgs::Manifold&)> getCurrentOccurenceCount =
[&traversalOccurences]
(const pgs::Manifold& manifold)
{
if (traversalOccurences.find(manifold.getInstanceId()) != traversalOccurences.end())
{
return traversalOccurences[manifold.getInstanceId()];
}

return 0;
};
std::function<void(const pgs::Manifold&)> incrementOccurenceCount =
[&traversalOccurences]
(const pgs::Manifold& manifold)
{
if (traversalOccurences.find(manifold.getInstanceId()) != traversalOccurences.end())
{
traversalOccurences[manifold.getInstanceId()] += 1;
}
else
{
traversalOccurences[manifold.getInstanceId()] = 1;
}
};

std::vector<const pgs::Manifold*> planarManifold;

// This lambda converts a manifold tree to a std::vector of its leaf
// which should all be elementary manifolds.
Expand Down Expand Up @@ -250,14 +250,21 @@ namespace roboptim
const pgs::Manifold* myManifold = planarManifold[static_cast<size_t>(index)];
bool sameType = myManifold->getTypeId() == manifold.getTypeId();
bool isNotRealSpace = myManifold->getTypeId() != pgs::RealSpace(1).getTypeId();
bool sameSize = getRestriction(*myManifold, getCurrentOccurenceCount(*myManifold), 0).second == manifold.representationDim();
long size1 = getRestriction(*myManifold, getCurrentOccurenceCount(*myManifold), 0).second;
long size2 = manifold.representationDim();
bool sameSize = size1 == size2;
incrementOccurenceCount(*myManifold);

if (sameType && (isNotRealSpace || sameSize))
{
return ++index;
}

if (sameType && !isNotRealSpace && !sameSize)
{
std::cerr << "RealSpaces must be of same size: Expecting "
<< size1 << " but got " << size2 << " instead." << std::endl;
}
return -1l;
}

Expand Down Expand Up @@ -396,18 +403,18 @@ namespace roboptim
return static_cast<int>(startIndex);
};

// Clear the traversalOccurences map to ensure a clean use of
// the getCurrentOccurenceCount and incrementOccurenceCount
// helper lambda functions.
traversalOccurences.clear();
// Computes the mapping
// Clear the traversalOccurences map to ensure a clean use of
// the getCurrentOccurenceCount and incrementOccurenceCount
// helper lambda functions.
traversalOccurences.clear();
// Computes the mapping
traverseFunctionManifold(functionManifold, 0);

// Clear the traversalOccurences map to ensure a clean use of
// the getCurrentOccurenceCount and incrementOccurenceCount
// helper lambda functions.
traversalOccurences.clear();
onTangentSpace = true;
// Clear the traversalOccurences map to ensure a clean use of
// the getCurrentOccurenceCount and incrementOccurenceCount
// helper lambda functions.
traversalOccurences.clear();
onTangentSpace = true;
traverseFunctionManifold(functionManifold, 0);
}

Expand Down Expand Up @@ -444,12 +451,12 @@ namespace roboptim
template <typename U>
void
FunctionOnManifold<U>::unmapTangentJacobian(pgs::RefMat jacobian)
const
const
{
for (long i = 0; i < this->tangentMappingFromFunctionSize_; ++i)
{
jacobian.col(static_cast<long>(this->tangentMappingFromFunction_[i])) = this->tangentMappedJacobian.col(i);
}
{
jacobian.col(static_cast<long>(this->tangentMappingFromFunction_[i])) = this->tangentMappedJacobian.col(i);
}
}

template <typename U>
Expand All @@ -465,8 +472,8 @@ namespace roboptim
template <typename U>
void
FunctionOnManifold<U>::impl_gradient (gradient_ref gradient,
const_argument_ref argument,
size_type functionId)
const_argument_ref argument,
size_type functionId)
const
{
this->mapArgument(argument);
Expand All @@ -479,7 +486,7 @@ namespace roboptim
template <typename U>
void
FunctionOnManifold<U>::impl_jacobian (jacobian_ref jacobian,
const_argument_ref argument)
const_argument_ref argument)
const
{
this->mapArgument(argument);
Expand All @@ -496,7 +503,7 @@ namespace roboptim
void
FunctionOnManifold<U>::manifold_jacobian (pgs::RefMat jacobian,
const_argument_ref argument)\
const
const
{
this->mapArgument(argument);
jacobian.setZero();
Expand Down
2 changes: 1 addition & 1 deletion include/roboptim/core/manifold-map/decorator/manifold-map.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ namespace roboptim
/// \param function the function instance.
static pgs::Manifold* getInstance(FI* function)
{
return new pgs::RealSpace(function->getSize());
return new pgs::RealSpace(function->inputSize());
}
};

Expand Down
5 changes: 3 additions & 2 deletions include/roboptim/core/manifold-map/decorator/problem-factory.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,8 +214,9 @@ ProblemFactory<U>::ProblemFactory()
{
this->objLambda_ = [](pgs::CartesianProduct& globMani)
{
typename GenericConstantFunction<EigenMatrixDense>::vector_t offset (globMani.representationDim());
GenericConstantFunction<EigenMatrixDense>* cst = new GenericConstantFunction<EigenMatrixDense>(offset);
typename GenericConstantFunction<EigenMatrixDense>::vector_t offset (1);
offset.setZero();
GenericConstantFunction<EigenMatrixDense>* cst = new GenericConstantFunction<EigenMatrixDense>(globMani.representationDim(),offset);

// We make a pgs::Manifold& out of the CartesianProduct& to explicitly call
// the overloaded constructor instead of the variadic one
Expand Down

0 comments on commit bbf7125

Please sign in to comment.