| 117 | } |
| 118 | |
| 119 | void BsplineOptimizer::optimize(Eigen::MatrixXd& points, double& dt, const int& cost_function, |
| 120 | const int& max_num_id, const int& max_time_id) { |
| 121 | if (start_state_.empty()) { |
| 122 | ROS_ERROR("Initial state undefined!"); |
| 123 | return; |
| 124 | } |
| 125 | control_points_ = points; |
| 126 | knot_span_ = dt; |
| 127 | max_num_id_ = max_num_id; |
| 128 | max_time_id_ = max_time_id; |
| 129 | setCostFunction(cost_function); |
| 130 | |
| 131 | // Set necessary data and flag |
| 132 | dim_ = control_points_.cols(); |
| 133 | if (dim_ == 1) |
| 134 | order_ = 3; |
| 135 | else |
| 136 | order_ = bspline_degree_; |
| 137 | point_num_ = control_points_.rows(); |
| 138 | optimize_time_ = cost_function_ & MINTIME; |
| 139 | variable_num_ = optimize_time_ ? dim_ * point_num_ + 1 : dim_ * point_num_; |
| 140 | if (variable_num_ <= 0) { |
| 141 | ROS_ERROR("Empty varibale to optimization solver."); |
| 142 | return; |
| 143 | } |
| 144 | |
| 145 | pt_dist_ = 0.0; |
| 146 | for (int i = 0; i < control_points_.rows() - 1; ++i) { |
| 147 | pt_dist_ += (control_points_.row(i + 1) - control_points_.row(i)).norm(); |
| 148 | } |
| 149 | pt_dist_ /= double(point_num_); |
| 150 | |
| 151 | iter_num_ = 0; |
| 152 | min_cost_ = std::numeric_limits<double>::max(); |
| 153 | g_q_.resize(point_num_); |
| 154 | g_smoothness_.resize(point_num_); |
| 155 | g_distance_.resize(point_num_); |
| 156 | g_feasibility_.resize(point_num_); |
| 157 | g_start_.resize(point_num_); |
| 158 | g_end_.resize(point_num_); |
| 159 | g_guide_.resize(point_num_); |
| 160 | g_waypoints_.resize(point_num_); |
| 161 | g_view_.resize(point_num_); |
| 162 | g_time_.resize(point_num_); |
| 163 | g_swarm_.resize(point_num_); |
| 164 | |
| 165 | comb_time = 0.0; |
| 166 | plan_start_time_ = ros::Time::now().toSec(); |
| 167 | |
| 168 | optimize(); |
| 169 | |
| 170 | points = control_points_; |
| 171 | dt = knot_span_; |
| 172 | start_state_.clear(); |
| 173 | time_lb_ = -1; |
| 174 | } |
| 175 | |
| 176 | void BsplineOptimizer::optimize() { |