Commit 9384d0d1 authored by Steve T's avatar Steve T
Browse files

implemented cross product for linear variable points

parent 9b6a391d
......@@ -35,5 +35,18 @@ void PseudoInverse(_Matrix_Type_& pinvmat) {
}
pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
}
template<typename Matrix3, typename Point >
Matrix3 skew(const Point& x) {
Matrix3 res = Matrix3::Zero(3,3);
res(0, 1) = -x(2);
res(0, 2) = x(1);
res(1, 0) = x(2);
res(1, 2) = -x(0);
res(2, 0) = -x(1);
res(2, 1) = x(0);
return res;
}
} // namespace curves
#endif //_SPLINEMATH
......@@ -509,19 +509,20 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
//http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
assert_operator_compatible(g);
if (dim()!= 3)
throw std::invalid_argument("Can't perform cross product polynomials with dimensions != 3 ");
throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
int m =(int)(degree());
int n =(int)(g.degree());
unsigned int mj, n_ij, mn_i;
t_point_t new_waypoints;
for(int i = 0; i<= m+n; ++i)
{
bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(3);
bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim());
for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
unsigned int mj = bin(m,j);
unsigned int n_ij = bin(n,i-j);
unsigned int mn_i = bin(m+n,i);
mj = bin(m,j);
n_ij = bin(n,i-j);
mn_i = bin(m+n,i);
num_t mul = num_t(mj*n_ij) / num_t(mn_i);
current_point += mul*curves::cross<bezier_curve_t::point_t>(waypointAtIndex(j), g.waypointAtIndex(i-j));
current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j));
}
new_waypoints.push_back(current_point);
}
......
......@@ -11,11 +11,26 @@
#include "curves/fwd.h"
namespace curves {
template<typename Point>
Eigen::Vector3d cross(const Point& a, const Point& b){
inline
Eigen::Vector3d cross(const Eigen::VectorXd& a, const Eigen::VectorXd& b){
Eigen::Vector3d c;
c << a[1]*b[2] - a[2]*b[1], a[2]*b[0] - a[0]*b[2], a[0]*b[1] - a[1]*b[0] ;
return c;
}
inline
Eigen::Vector3d cross(const Eigen::Vector3d& a, const Eigen::Vector3d& b){
return a.cross(b);
}
inline
Eigen::Vector3f cross(const Eigen::Vector3f& a, const Eigen::Vector3f& b){
return a.cross(b);
}
template<typename N, bool S>
linear_variable<N,S> cross(const linear_variable<N,S>& a, const linear_variable<N,S>& b){
return a.cross(b);
}
} // namespace curves
#endif //_CLASS_CROSSIMP
......@@ -87,6 +87,8 @@ typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t, t_pointX_t, polynomial_t> exact_cubic_t;
typedef bezier_curve<double, double, true, pointX_t> bezier_t;
typedef linear_variable<double, true> linear_variable_t;
typedef bezier_curve<double, double, true, linear_variable_t> bezier_linear_variable_t;
typedef constant_curve<double, double, true, pointX_t, pointX_t> constant_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve<double, double, true, pointX_t, pointX_t, curve_abc_t> piecewise_t;
......
......@@ -26,11 +26,14 @@ template <typename Numeric = double, bool Safe = true>
struct linear_variable : public serialization::Serializable {
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
typedef Eigen::Matrix<Numeric, 3,1> vector_3_t;
typedef Eigen::Matrix<Numeric, 3,3> matrix_3_t;
typedef linear_variable<Numeric> linear_variable_t;
linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable
linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
linear_variable(const linear_variable_t& other) : B_(other.B()), c_(other.c()), zero(other.isZero()) {} // copy constructor
/// \brief Linear evaluation for vector x.
/// \param val : vector to evaluate the linear variable.
......@@ -95,12 +98,34 @@ struct linear_variable : public serialization::Serializable {
return *this;
}
/// \brief Compute the cross product of the current linear_variable and the other.
/// This method of course only makes sense for dimension 3 curves and dimension 3 unknown,
/// since otherwise the result is non-linear.
/// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
/// \param pOther other polynomial to compute the cross product with.
/// \return a new polynomial defining the cross product between this and other
linear_variable_t cross(const linear_variable_t& other) const {
if (B().rows() !=3)
throw std::invalid_argument("Can't perform cross product on linear variables with dimensions != 3 ");
if (B().cols() !=3)
throw std::invalid_argument("Can't perform cross product on linear variables more than one unknown ");
if (isZero() || other.isZero())
return linear_variable_t::Zero(3);
if ((B().squaredNorm() - B().diagonal().squaredNorm() > linear_variable_t::MARGIN ) || (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > linear_variable_t::MARGIN ) )
throw std::invalid_argument("Can't perform cross product on linear variables if B is not diagonal ");
// (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2
typename linear_variable_t::matrix_3_t newB = skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(-other.c()) * B() +
skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(c()) * other.B();
typename linear_variable_t::vector_3_t newC = curves::cross(c(),other.c());
return linear_variable_t(newB,newC);
}
/// \brief Get a linear variable equal to zero.
/// \param dim : Dimension of linear variable.
/// \return Linear variable equal to zero.
///
static linear_variable_t Zero(size_t dim = 0) {
return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim));
}
/// \brief Get dimension of linear variable.
......@@ -142,6 +167,7 @@ struct linear_variable : public serialization::Serializable {
matrix_x_t B_;
vector_x_t c_;
bool zero;
static const double MARGIN;
};
template <typename N, bool S>
......@@ -187,6 +213,14 @@ BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
}
template <typename N, bool S>
std::ostream &operator<<(std::ostream &os, const linear_variable<N, S>& l) {
return os << "linear_variable: \n \t B:\n"<< l.B() << "\t c: \n" << l.c().transpose();
}
template<typename N, bool S>
const double linear_variable<N,S>::MARGIN(0.001);
} // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Numeric, bool Safe),
......
......@@ -448,7 +448,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
polynomial_t cross(const polynomial_t& pOther) const {
assert_operator_compatible(pOther);
if (dim()!= 3)
throw std::invalid_argument("Can't perform cross product polynomials with dimensions != 3 ");
throw std::invalid_argument("Can't perform cross product on polynomials with dimensions != 3 ");
std::size_t new_degree =degree() + pOther.degree();
coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+1);
Eigen::Vector3d currentVec;
......
......@@ -730,7 +730,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
.def("size", &linear_variable_t::size)
.def("isZero", &linear_variable_t::isZero)
.def("norm", &linear_variable_t::norm);
.def("norm", &linear_variable_t::norm)
.def("cross", &linear_variable_t::cross,bp::args("other"),"Compute the cross product of the current linear_variable and the other. Only works for dimension 3");
class_<bezier_linear_variable_t, bases<curve_abc_t>, boost::shared_ptr<bezier_linear_variable_t> >(
"bezier_linear_variable", no_init)
......@@ -749,6 +750,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
.def_readonly("degree", &bezier_linear_variable_t::degree_)
.def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
.def("cross", &bezier_linear_variable_t::cross,bp::args("other"),"Compute the cross product of the current bezier_linear_variable and the other. Only works for dimension 3")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += bezier_linear_variable_t())
......
......@@ -108,6 +108,55 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
}
BOOST_AUTO_TEST_CASE(crossPoductLinearVariable, * boost::unit_test::tolerance(0.001)) {
linear_variable_t l1(Eigen::Matrix3d::Identity() * 5., Eigen::Vector3d::Random());
linear_variable_t l2(Eigen::Matrix3d::Identity() * 1., Eigen::Vector3d::Random());
linear_variable_t lE(Eigen::Matrix3d::Random(), Eigen::Vector3d::Random());
BOOST_CHECK_THROW( l1.cross(lE), std::exception );
BOOST_CHECK_THROW( lE.cross(l1), std::exception );
linear_variable_t lcross = l1.cross(l2);
for (int i =0; i< 10; ++i){
Eigen::Vector3d x = Eigen::Vector3d::Random();
Eigen::Vector3d v1 = l1(x);
Eigen::Vector3d v2 = l2(x);
BOOST_TEST(( lcross(x) - v1.cross(v2)).norm()==0.);
}
}
BOOST_AUTO_TEST_CASE(crossProductBezierLinearVariable, * boost::unit_test::tolerance(0.001)) {
bezier_linear_variable_t::t_point_t vec1;
bezier_linear_variable_t::t_point_t vec2;
bezier_linear_variable_t::t_point_t zeroVec;
for (int i =0; i<3; ++i)
{
vec1.push_back(linear_variable_t(Eigen::Matrix3d::Identity() * i, Eigen::Vector3d::Random()));
vec2.push_back(linear_variable_t(Eigen::Matrix3d::Identity() * i, Eigen::Vector3d::Random()));
zeroVec.push_back(linear_variable_t());
}
for (int i =0; i<2; ++i)
{
vec1.push_back(linear_variable_t(Eigen::Matrix3d::Identity() * i, Eigen::Vector3d::Random()));
}
bezier_linear_variable_t p1(vec1.begin(),vec1.end(),0.,1.);
bezier_linear_variable_t p2(vec2.begin(),vec2.end(),0.,1.);
bezier_linear_variable_t pZero(zeroVec.begin(),zeroVec.end(),0.,1.);
bezier_linear_variable_t pCross (p1.cross(p2));
bezier_linear_variable_t pCrossZero (p1.cross(pZero));
for (double i = 0.; i <=1.; ++i ){
Eigen::Vector3d x = Eigen::Vector3d::Random();
bezier_t fcross = evaluateLinear<bezier_t, bezier_linear_variable_t>(pCross, x);
bezier_t fCrossZero = evaluateLinear<bezier_t, bezier_linear_variable_t>(pCrossZero, x);
bezier_t f1 = evaluateLinear<bezier_t, bezier_linear_variable_t>(p1, x);
bezier_t f2 = evaluateLinear<bezier_t, bezier_linear_variable_t>(p2, x);
for (double i = 0.; i <=10.; ++i ){
double dt = i / 10.;
Eigen::Vector3d v1 = f1(dt);
Eigen::Vector3d v2 = f2(dt);
BOOST_TEST(( fcross(dt) - v1.cross(v2)).norm()==0.);
BOOST_TEST(( fCrossZero(dt)).norm()==0.);
}
}
}
BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001)) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment