Measure/Modules/TestGTSAM/edge3_test/Edge3Factor.cpp

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

#include "Edge3Factor.h"

Vector Edge3Factor::evaluateError(const double& x, const double& y, const double& z,
              boost::optional<Matrix&> H1,
              boost::optional<Matrix&> H2,
              boost::optional<Matrix&> H3) const {

  if (H1) {
    *H1 = (Matrix(2, 1) << (z - pb_ * y) / (x * x), pa_ / y).finished();
  }
  if (H2) {
    *H2 = (Matrix(2, 1) << pb_/x, (z-pa_*x)/(y*y)).finished();
  }
  if (H3) {
    *H3 = (Matrix(2, 1) << -1.0/x, -1.0 / y).finished();
  }
  return (Vector(2)<<pa_-(z-pb_*y)/x,pb_-(z-pa_*x)/y).finished();

}


Vector Edge3Factor2::evaluateError(const double& x, const double& y, const double& z,
                                  boost::optional<Matrix&> H1,
                                  boost::optional<Matrix&> H2,
                                  boost::optional<Matrix&> H3) const {

  if (H1) {
    *H1 = (Matrix(2, 1) << (z - pb_ * y) / (x * x), pa_ / y).finished();
  }
  if (H2) {
    *H2 = (Matrix(2, 1) << pb_/x, (z-pa_*x)/(y*y)).finished();
  }
  if (H3) {
    *H3 = (Matrix(2, 1) << -1.0/x, -1.0 / y).finished();
  }
  return (Vector(2)<<pa_-(z-pb_*y)/x,pb_-(z-pa_*x)/y).finished();

}