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():表示两个向量求距离,开根号.
