Skip to content

Commit

Permalink
Merge pull request #419 from YoshuaNava/feature/transformations_inpla…
Browse files Browse the repository at this point in the history 
…ce_compute

Implemented an in-place method for transforming DataPoints objects
  • Loading branch information
@pomerlef
pomerlef committed Dec 1, 2020
2 parents e4375a3 + 3e65d06 commit fba9de4
Show file tree
Hide file tree
Showing 7 changed files with 451 additions and 139 deletions.
3 changes: 3 additions & 0 deletions pointmatcher/PointMatcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -410,6 +410,9 @@ struct PointMatcher
//! Transform input using the transformation matrix
virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0;

//! Transform point cloud in-place using the transformation matrix
virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const = 0;

//! Return whether the given parameters respect the expected constraints
virtual bool checkParameters(const TransformationParameters& parameters) const = 0;

Expand Down
1 change: 1 addition & 0 deletions pointmatcher/Registry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ PointMatcher<T>::PointMatcher()
{
ADD_TO_REGISTRAR_NO_PARAM(Transformation, RigidTransformation, typename TransformationsImpl<T>::RigidTransformation)
ADD_TO_REGISTRAR_NO_PARAM(Transformation, PureTranslation, typename TransformationsImpl<T>::PureTranslation)
ADD_TO_REGISTRAR_NO_PARAM(Transformation, SimilarityTransformation, typename TransformationsImpl<T>::SimilarityTransformation)

ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, IdentityDataPointsFilter, typename DataPointsFiltersImpl<T>::IdentityDataPointsFilter)
ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, RemoveNaNDataPointsFilter, typename DataPointsFiltersImpl<T>::RemoveNaNDataPointsFilter)
Expand Down
5 changes: 1 addition & 4 deletions pointmatcher/Transformation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,12 +66,9 @@ void PointMatcher<T>::Transformations::apply(DataPoints& cloud, const Transforma
// TODO: This API must be re-written to not even have the concept
// of chain for this classs.
int num_iter = 0;

DataPoints transformedCloud;
for (TransformationsConstIt it = this->begin(); it != this->end(); ++it)
{
transformedCloud = (*it)->compute(cloud, parameters);
swapDataPoints(cloud, transformedCloud);
(*it)->inPlaceCompute(parameters, cloud);
num_iter++;
}
if (num_iter != 1)
Expand Down
88 changes: 52 additions & 36 deletions pointmatcher/TransformationsImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,18 @@ typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation
const DataPoints& input,
const TransformationParameters& parameters) const
{
assert(input.features.rows() == parameters.rows());
DataPoints transformedCloud = input;
inPlaceCompute(parameters, transformedCloud);
return transformedCloud;
}

//! RigidTransformation
template<typename T>
void TransformationsImpl<T>::RigidTransformation::inPlaceCompute(
const TransformationParameters& parameters,
DataPoints& cloud) const
{
assert(cloud.features.rows() == parameters.rows());
assert(parameters.rows() == parameters.cols());

const unsigned int nbRows = parameters.rows()-1;
Expand All @@ -62,28 +73,23 @@ typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation
if(this->checkParameters(parameters) == false)
throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");

//DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols());
DataPoints transformedCloud = input;

// Apply the transformation to features
transformedCloud.features = parameters * input.features;
cloud.features.applyOnTheLeft(parameters);

// Apply the transformation to descriptors
int row(0);
const int descCols(input.descriptors.cols());
for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
const int descCols(cloud.descriptors.cols());
for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i)
{
const int span(input.descriptorLabels[i].span);
const std::string& name(input.descriptorLabels[i].text);
const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
const int span(cloud.descriptorLabels[i].span);
const std::string& name(cloud.descriptorLabels[i].text);
if (name == "normals" || name == "observationDirections")
outputDesc = R * inputDesc;
{
cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R);
}

row += span;
}

return transformedCloud;
}

//! Ensure orthogonality of the rotation matrix
Expand Down Expand Up @@ -159,7 +165,18 @@ typename PointMatcher<T>::DataPoints TransformationsImpl<T>::SimilarityTransform
const DataPoints& input,
const TransformationParameters& parameters) const
{
assert(input.features.rows() == parameters.rows());
DataPoints transformedCloud = input;
inPlaceCompute(parameters, transformedCloud);
return transformedCloud;
}

//! SimilarityTransformation
template<typename T>
void TransformationsImpl<T>::SimilarityTransformation::inPlaceCompute(
const TransformationParameters& parameters,
DataPoints& cloud) const
{
assert(cloud.features.rows() == parameters.rows());
assert(parameters.rows() == parameters.cols());

const unsigned int nbRows = parameters.rows()-1;
Expand All @@ -170,28 +187,23 @@ typename PointMatcher<T>::DataPoints TransformationsImpl<T>::SimilarityTransform
if(this->checkParameters(parameters) == false)
throw TransformationError("SimilarityTransformation: Error, invalid similarity transform.");

//DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols());
DataPoints transformedCloud = input;

// Apply the transformation to features
transformedCloud.features = parameters * input.features;
cloud.features.applyOnTheLeft(parameters);

// Apply the transformation to descriptors
int row(0);
const int descCols(input.descriptors.cols());
for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
const int descCols(cloud.descriptors.cols());
for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i)
{
const int span(input.descriptorLabels[i].span);
const std::string& name(input.descriptorLabels[i].text);
const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
const int span(cloud.descriptorLabels[i].span);
const std::string& name(cloud.descriptorLabels[i].text);
if (name == "normals" || name == "observationDirections")
outputDesc = R * inputDesc;
{
cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R);
}

row += span;
}

return transformedCloud;
}

//! Nothing to check for a similarity transform
Expand All @@ -215,19 +227,23 @@ template struct TransformationsImpl<double>::SimilarityTransformation;
template<typename T>
typename PointMatcher<T>::DataPoints TransformationsImpl<T>::PureTranslation::compute(const DataPoints& input,
const TransformationParameters& parameters) const {
assert(input.features.rows() == parameters.rows());
DataPoints transformedCloud = input;
inPlaceCompute(parameters, transformedCloud);
return transformedCloud;
}

template<typename T>
void TransformationsImpl<T>::PureTranslation::inPlaceCompute(
const TransformationParameters& parameters,
DataPoints& cloud) const {
assert(cloud.features.rows() == parameters.rows());
assert(parameters.rows() == parameters.cols());

if(this->checkParameters(parameters) == false)
throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity.");

//DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.features.cols());
DataPoints transformedCloud = input;

// Apply the transformation to features
transformedCloud.features = parameters * input.features;

return transformedCloud;
cloud.features.applyOnTheLeft(parameters);
}

template<typename T>
Expand Down
4 changes: 4 additions & 0 deletions pointmatcher/TransformationsImpl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ struct TransformationsImpl

RigidTransformation() : Transformation("RigidTransformation", ParametersDoc(), Parameters()) {}
virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const;
virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const;
virtual bool checkParameters(const TransformationParameters& parameters) const;
virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const;
};
Expand All @@ -71,7 +72,9 @@ struct TransformationsImpl
return "Similarity transformation (rotation + translation + scale).";
}

SimilarityTransformation() : Transformation("SimilarityTransformation", ParametersDoc(), Parameters()) {}
virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const;
virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const;
virtual bool checkParameters(const TransformationParameters& parameters) const;
virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const;
};
Expand All @@ -85,6 +88,7 @@ struct TransformationsImpl

PureTranslation() : Transformation("PureTranslation", ParametersDoc(), Parameters()) {}
virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const;
virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const;
virtual bool checkParameters(const TransformationParameters& parameters) const;
virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const;
};
Expand Down
Loading

0 comments on commit fba9de4

Please sign in to comment.