ref_affine矩阵含义

它的具体实现是这样的,再详细解释一下 bool Spline2dConstraint::Add2dBoundary( const std::vector<double>& t_coord, const std::vector<double>& angle, const std::vector<Vec2d>& ref_point, const std::vector<double>& longitudinal_bound, const std::vector<double>& lateral_bound) { if (t_coord.size() != angle.size() || angle.size() != ref_point.size() || ref_point.size() != lateral_bound.size() || lateral_bound.size() != longitudinal_bound.size()) { return false; } Eigen::MatrixXd affine_inequality = Eigen::MatrixXd::Zero(4 * t_coord.size(), total_param_); Eigen::MatrixXd affine_boundary = Eigen::MatrixXd::Zero(4 * t_coord.size(), 1); for (uint32_t i = 0; i < t_coord.size(); ++i) { const double d_lateral = SignDistance(ref_point[i], angle[i]); const double d_longitudinal = SignDistance(ref_point[i], angle[i] - M_PI / 2.0); const uint32_t index = FindIndex(t_coord[i]); const double rel_t = t_coord[i] - t_knots_[index]; const uint32_t index_offset = 2 * index * (spline_order_ + 1); std::vector<double> longi_coef = AffineCoef(angle[i], rel_t); std::vector<double> longitudinal_coef = AffineCoef(angle[i] - M_PI / 2, rel_t); for (uint32_t j = 0; j < 2 * (spline_order_ + 1); ++j) { // upper longi affine_inequality(4 * i, index_offset + j) = longi_coef[j]; // lower longi affine_inequality(4 * i + 1, index_offset + j) = -longi_coef[j]; // upper longitudinal affine_inequality(4 * i + 2, index_offset + j) = longitudinal_coef[j]; // lower longitudinal affine_inequality(4 * i + 3, index_offset + j) = -longitudinal_coef[j]; } affine_boundary(4 * i, 0) = d_lateral - lateral_bound[i]; affine_boundary(4 * i + 1, 0) = -d_lateral - lateral_bound[i]; affine_boundary(4 * i + 2, 0) = d_longitudinal - longitudinal_bound[i]; affine_boundary(4 * i + 3, 0) = -d_longitudinal - longitudinal_bound[i]; } // std::cout << affine_inequality << std::endl; return AddInequalityConstraint(affine_inequality, affine_boundary); }
最新发布
07-11
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值