slam里面G-N实现Bundle Adjustment中位姿估计,二范数求误差累积.
二范数:一对误差的平方
Vector2d e=p2-p2_;
double cost_squarednorm=e.squaredNorm(); //求二范数;
cost +=cost_squarednorm
二.
Eigen::Vector3d trajectory_end(trajectory.back().x, trajectory.back().y, trajectory.back().yaw);
float goal_cost = (trajectory_end.segment(0,2) - goal.segment(0,2)).norm();
segment(0,2):表示取向量多少个元素.
norm():表示两个向量求距离,开根号.