//
// Created by zx on 23-6-20.
//

#include "PoseSpeedFactor.h"


Vector PoseSpeedFactor::evaluateError(const Pose2& p1, const Vector2 & speed1, const Pose2& p2,
                     boost::optional<Matrix&> H1,
                     boost::optional<Matrix&> H2,
                     boost::optional<Matrix&> H3) const{
  double v1=speed1[0];
  double w1=speed1[1];
  double x1=p1.x();
  double y1=p1.y();
  double th1=p1.theta();
  double x2=p2.x();
  double y2=p2.y();
  double th2=p2.theta();
  //loss 对 p1求导
  if(H1){
    *H1 = (Matrix(3, 3) << ( 0, 0, 1.0/w1,
                            1.0/(cos(th1)*v1), 0, -(x2-x1)*v1*sin(th1)/pow(cos(th1)*v1,2),
                            0, 1.0/(sin(th1)*v1), (y2-y1)*v1*cos(th1)/pow(sin(th1)*v1,2) )).finished();
  }
  if(H2){
    *H2 = (Matrix(3, 2) << ( 0, (th2-th1)/(w1*w1),
        (x2-x1)*cos(th1)/pow(cos(th1)*v1,2),0,
        (y2-y1)*sin(th1)/pow(sin(th1)*v1,2),0 )).finished();
  }
  if(H3){
    *H3 = (Matrix(3, 3) << ( 0, 0,0,
        -1.0/(cos(th1)*v1), 0, 0,
        0, -1.0/(sin(th1)*v1), 0 )).finished();
  }



  return (Vector(3)<<dt_[0]-(th2-th1)/w1 ,dt_[1]-(x2-x1)/(cos(th1)*v1), dt_[2]-(y2-y1)/(sin(th1)*v1) ).finished();


}