Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Feature/4 dof for point to plane minimizer #378

Merged
merged 2 commits into from
May 11, 2020
Merged

Feature/4 dof for point to plane minimizer #378

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d1c0cb6
Adding the ability to force 4DOF minimization in the point-to-plane.
kubelvla Oct 8, 2019
dac934c
A unit test added for 4DOF point-to-plane minimizer.
kubelvla Apr 30, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions examples/data/icp_data/force4DOFForPointToPlaneMinimizer.ref_trans
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
0.9836155772209167 -0.1802777200937271 0 0.2438895702362061
0.1802777200937271 0.9836155772209167 0 -0.709206759929657
0 0 1 -0.1967320442199707
0 0 0 1
53 changes: 53 additions & 0 deletions examples/data/icp_data/force4DOFForPointToPlaneMinimizer.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
readingDataPointsFilters:
- SurfaceNormalDataPointsFilter:
knn: 12
epsilon: 1.33
keepNormals: 1
keepDensities: 1

referenceDataPointsFilters:
- SurfaceNormalDataPointsFilter:
knn: 15

- ObservationDirectionDataPointsFilter

- OrientNormalsDataPointsFilter:
towardCenter: 1

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are the extra lines between the reference data points filters intended?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like it better, easier to distinguish between the filters.


matcher:
KDTreeMatcher:
knn: 10
maxDist: 1.5
epsilon: 1

outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.80

- SurfaceNormalOutlierFilter:
maxAngle: 0.42

errorMinimizer:
PointToPlaneErrorMinimizer:
force4DOF: 1

transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 2
- CounterTransformationChecker:
maxIterationCount: 100
- BoundTransformationChecker:
maxRotationNorm: 0.5
maxTranslationNorm: 2 #1

inspector:
NullInspector
# VTKFileInspector:

logger:
NullLogger
# FileLogger

84 changes: 77 additions & 7 deletions pointmatcher/ErrorMinimizers/PointToPlane.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,17 +53,55 @@ typedef Parametrizable::ParametersDoc ParametersDoc;
template<typename T>
PointToPlaneErrorMinimizer<T>::PointToPlaneErrorMinimizer(const Parameters& params):
ErrorMinimizer(name(), availableParameters(), params),
force2D(Parametrizable::get<T>("force2D"))
force2D(Parametrizable::get<T>("force2D")),
force4DOF(Parametrizable::get<T>("force4DOF"))
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
if(force2D)
{
if (force4DOF)
{
throw PointMatcherSupport::ConfigurationError("Force 2D cannot be used together with force4DOF.");
}
else
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
}
}
else if(force4DOF)
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 4-DOF (yaw,x,y,z).");
}
else
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 3D.");
}
}

template<typename T>
PointToPlaneErrorMinimizer<T>::PointToPlaneErrorMinimizer(const ParametersDoc paramsDoc, const Parameters& params):
ErrorMinimizer(name(), paramsDoc, params),
force2D(Parametrizable::get<T>("force2D"))
force2D(Parametrizable::get<T>("force2D")),
force4DOF(Parametrizable::get<T>("force4DOF"))
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
if(force2D)
{
if (force4DOF)
{
throw PointMatcherSupport::ConfigurationError("Force 2D cannot be used together with force4DOF.");
}
else
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 2D.");
}
}
else if(force4DOF)
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 4-DOF (yaw,x,y,z).");
}
else
{
LOG_INFO_STREAM("PointMatcher::PointToPlaneErrorMinimizer - minimization will be in 3D.");
}
}


Expand Down Expand Up @@ -154,7 +192,23 @@ typename PointMatcher<T>::TransformationParameters PointToPlaneErrorMinimizer<T>
assert(normalRef.rows() > 0);

// Compute cross product of cross = cross(reading X normalRef)
const Matrix cross = this->crossProduct(mPts.reading.features, normalRef);
Matrix cross;
Matrix matrixGamma(3,3);
if(!force4DOF)
{
// Compute cross product of cross = cross(reading X normalRef)
cross = this->crossProduct(mPts.reading.features, normalRef);
}
else
{
//VK: Instead for "cross" as in 3D, we need only a dot product with the matrixGamma factor for 4DOF
//VK: This should be published in 2020 or 2021
matrixGamma << 0,-1, 0,
1, 0, 0,
0, 0, 0;
cross = ((matrixGamma*mPts.reading.features).transpose()*normalRef).diagonal().transpose();
}


// wF = [weights*cross, weights*normals]
// F = [cross, normals]
Expand Down Expand Up @@ -203,14 +257,30 @@ typename PointMatcher<T>::TransformationParameters PointToPlaneErrorMinimizer<T>
* Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
* Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ());*/

transform = Eigen::AngleAxis<T>(x.head(3).norm(),x.head(3).normalized());
// Normal 6DOF takes the whole rotation vector from the solution to construct the output quaternion
if (!force4DOF)
{
transform = Eigen::AngleAxis<T>(x.head(3).norm(), x.head(3).normalized()); //x=[alpha,beta,gamma,x,y,z]
} else // 4DOF needs only one number, the rotation around the Z axis
{
Vector unitZ(3,1);
unitZ << 0,0,1;
transform = Eigen::AngleAxis<T>(x(0), unitZ); //x=[gamma,x,y,z]
}

// Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
/*const T pitch = -asin(transform(2,0));
const T roll = atan2(transform(2,1), transform(2,2));
const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
transform.translation() = x.segment(3, 3);
if (!force4DOF)
{
transform.translation() = x.segment(3, 3); //x=[alpha,beta,gamma,x,y,z]
} else
{
transform.translation() = x.segment(1, 3); //x=[gamma,x,y,z]
}

mOut = transform.matrix();

if (mOut != mOut)
Expand Down
7 changes: 5 additions & 2 deletions pointmatcher/ErrorMinimizers/PointToPlane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,11 +69,14 @@ struct PointToPlaneErrorMinimizer: public PointMatcher<T>::ErrorMinimizer
inline static const ParametersDoc availableParameters()
{
return {
{"force2D", "If set to true(1), the minimization will be force to give a solution in 2D (i.e., on the XY-plane) even with 3D inputs.", "0", "0", "1", &P::Comp<bool>}
};
{"force2D", "If set to true(1), the minimization will be forced to give a solution in 2D (i.e., on the XY-plane) even with 3D inputs.", "0", "0", "1", &P::Comp<bool>},
{"force4DOF", "If set to true(1), the minimization will optimize only yaw and translation, pitch and roll will follow the prior.", "0", "0", "1", &P::Comp<bool>}

};
}

const bool force2D;
const bool force4DOF;

PointToPlaneErrorMinimizer(const Parameters& params = Parameters());
PointToPlaneErrorMinimizer(const ParametersDoc paramsDoc, const Parameters& params);
Expand Down
6 changes: 5 additions & 1 deletion pointmatcher/Exceptions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
template<typename T>
PointMatcher<T>::ConvergenceError::ConvergenceError(const std::string& reason):
runtime_error(reason)
{}
{}

PointMatcherSupport::ConfigurationError::ConfigurationError(const std::string& reason):
runtime_error(reason)
{}

template struct PointMatcher<float>::ConvergenceError;
template struct PointMatcher<double>::ConvergenceError;
8 changes: 8 additions & 0 deletions pointmatcher/PointMatcher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,14 @@ namespace PointMatcherSupport
//! return an exception when a transformation has invalid parameters
TransformationError(const std::string& reason);
};

//! An expception thrown when the yaml config file contains invalid configuration (e.g., mutually exclusive settings)
struct ConfigurationError: std::runtime_error
{
//! return an exception when a transformation has invalid parameters
ConfigurationError(const std::string& reason);
};


//! The logger interface, used to output warnings and informations
struct Logger: public Parametrizable
Expand Down