/**
* Computes the function `Y = f(X)` with `X = [x, y, z]` and `Y = [u, v]`.
*
* This function also computes the Jacobian `J` of `[u, v] = f(x, y, z)` as
* J = | du/dx du/dy du/dz|
* | dv/dx dv/dy dv/dz|.
*/
template<class Functor>
inline bool autoDiff(const Functor& func,
const Eigen::Vector3d& X,
Eigen::Vector2d& Y,
Eigen::Matrix<double, 2, 3>& J)
{
// Wrap input point double** as required by AutoDiff<> class
const double* parametersX = &X[0];
const double** parameters = ¶metersX;
// Init memory for Jacobian
double** jacobians = new double*[2];
for(size_t i=0; i<2; i++)
{
jacobians[i] = new double[3];
for(size_t j=0; j<3; j++) {
jacobians[i][j] = 0;
}
}
ceres::internal::AutoDiff<Functor, double, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0>
::Differentiate(func, parameters,
2*3, Y.data(), (double**) jacobians);
// Copy to output variable J
for(size_t i=0; i<2; i++)
for(size_t j=0; j<3; j++)
J(i, j) = jacobians[i][j];
for(size_t i=0; i<2; i++)
delete[] jacobians[i];
delete[] jacobians;
return true;
}
/**
* Functor to wrap the project() function.
*/
struct CameraProjectionModel
{
CameraProjectionModel(){}
CameraProjectionModel(const OCamModelParams* params)
: params_(params){}
template <typename T>
bool operator()(const T point3D[3], T point2D[2]) const
{
project(point3D, point2D, this->params_);
return true;
}
const CameraProjectionModelParams* params_;
};