Commit 9384d0d1 by Steve T

### 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 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 { //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(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 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 linear_variable cross(const linear_variable& a, const linear_variable& b){ return a.cross(b); } } // namespace curves #endif //_CLASS_CROSSIMP
 ... ... @@ -87,6 +87,8 @@ typedef boost::shared_ptr curve_SE3_ptr_t; typedef polynomial polynomial_t; typedef exact_cubic exact_cubic_t; typedef bezier_curve bezier_t; typedef linear_variable linear_variable_t; typedef bezier_curve bezier_linear_variable_t; typedef constant_curve constant_t; typedef cubic_hermite_spline cubic_hermite_spline_t; typedef piecewise_curve piecewise_t; ... ...
 ... ... @@ -26,11 +26,14 @@ template struct linear_variable : public serialization::Serializable { typedef Eigen::Matrix vector_x_t; typedef Eigen::Matrix matrix_x_t; typedef Eigen::Matrix vector_3_t; typedef Eigen::Matrix matrix_3_t; typedef linear_variable 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(-other.c()) * B() + skew(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 ... ... @@ -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 std::ostream &operator<<(std::ostream &os, const linear_variable& l) { return os << "linear_variable: \n \t B:\n"<< l.B() << "\t c: \n" << l.c().transpose(); } template const double linear_variable::MARGIN(0.001); } // namespace curves DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Numeric, bool Safe), ... ...
 ... ... @@ -448,7 +448,7 @@ struct polynomial : public curve_abc { 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()) .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_, boost::shared_ptr >( "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(pCross, x); bezier_t fCrossZero = evaluateLinear(pCrossZero, x); bezier_t f1 = evaluateLinear(p1, x); bezier_t f2 = evaluateLinear(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!