I am using ceres-solver with AutoDiffCostFunction. My cost function takes as parameter 1x3 vector and outputs 1x1 residual.
How can I create opencv Mat out of my T* parameter vector? It may be either Jet or float.
I tried following code, but get error "cannot conver from Jet to float"
struct ErrorFunc
{
template <typename T>
bool operator()(const T * const Kparams, T * residual) const // Kparams - [f, u, v]
{
cv::Mat K = cv::Mat::eye(3, 3, CV_32F);
K.at<float>(0, 0) = float(Kparams[0]); // error
K.at<float>(0, 2) = float(Kparams[1]); // error
K.at<float>(1, 1) = float(Kparams[0]); // error
K.at<float>(1, 2) = float(Kparams[2]); // error
Mat Hdot = K.inv() * H * K;
cv::decomposeHomographyMat(Hdot, K, rot, tr, norm); //want to call this opencv function
residual[0] = calcResidual(norm);
return true;
}
Mat H;
}
There is a way to get Eigen matrix out of T* matrix:
const Eigen::Matrix< T, 3, 3, Eigen::RowMajor> hom = Eigen::Map< const Eigen::Matrix< T, 3, 3, Eigen::RowMajor> >(Matrix)
but I want to call cv::decomposeHomographyMat . How can I do this?
You cannot use an OpenCV method in a ceres::AutoDiffCostFunction in this way. The OpenCV method is not templated with type T as required by ceres to do the automatic differentiation. The float cast cannot be done because the ceres jet of Jacobians is a vector and not a scalar.
You have two options:
1) Use numerical differentiation: see http://ceres-solver.org/nnls_tutorial.html#numeric-derivatives
2) Use a templated library (e.g. Eigen http://eigen.tuxfamily.org/index.php?title=Main_Page) to rewrite the required homography decomposition
Related
I get a vector<vector<Point>> data by OpenCV. For some reasons (for example offset/scaling), I need to convert the data Point to Point2f. How can I do that?
For example:
std::vector<std::vector<Point> > contours;
std::vector<std::Vec4i> hierarchy;
cv::findContours(edges, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
std::vector<std::vector<Point2f> > new_contour;
I need new_contour which is vector of vector of Point2f. Is there a simple way that convert it to float type? Then I can do some modification (for example offset/scaling) by replacing each Point2f data.
I try it using push_back. But still get error when building...
You can use 2 nested loops: one for the outer vector and one for the inner.
In the code below you can replace the trivial conversion between int and float and apply any transformation you need.
Note that for the output contours, I allocated the outer vector using std::vector::resize and then in a similar way allocated all the inner vectors in the loop.
Alternatively you could use std::vector::reserve to do all the allocations together with std::vector::push_back for adding the elements.
std::vector<std::vector<cv::Point>> contours;
// ... obtain the contours
std::vector<std::vector<cv::Point2f>> contours_float;
contours_float.resize(contours.size()); // allocate the outer vector
for (size_t i = 0; i < contours.size(); ++i)
{
auto const & cur_contour = contours[i];
auto & cur_contour_float = contours_float[i];
cur_contour_float.resize(cur_contour.size()); // allocate the current inner vector
for (size_t j = 0; j < cur_contour.size(); ++j)
{
auto const & cur_point = cur_contour[j];
// Here you can apply any transformation you need:
float x = static_cast<float>(cur_point.x);
float y = static_cast<float>(cur_point.y);
cur_contour_float[j] = cv::Point2f{ x,y };
}
}
Another way to implement this is using std::transform (here I found it convenient to allocate the vectors by using the appropriate std::vector constructor):
std::vector<std::vector<cv::Point>> contours;
// ... obtain the contours
std::vector<std::vector<cv::Point2f>> contours_float(contours.size()); // allocate the outer vector
std::transform(
contours.begin(),
contours.end(),
contours_float.begin(),
[](auto const & cur_contour) -> auto
{
std::vector<cv::Point2f> cur_contour_float(cur_contour.size()); // allocate the current inner vector
std::transform(
cur_contour.begin(),
cur_contour.end(),
cur_contour_float.begin(),
[](auto const & cur_point) -> auto
{
// Here you can apply any transformation you need:
float x = static_cast<float>(cur_point.x);
float y = static_cast<float>(cur_point.y);
return cv::Point2f{ x,y };
});
return cur_contour_float;
});
The 2nd version actually implements the exact operation that you require (transformation from one representation to another).
I'll first focus on your actual question, which is just the type conversion.
Let's say we have some basic type aliases to make our life easier:
using pi_vec = std::vector<cv::Point2i>; // NB: Point is an alias for Point2i
using pf_vec = std::vector<cv::Point2f>;
using pi_vec_vec = std::vector<pi_vec>;
using pf_vec_vec = std::vector<pf_vec>;
To solve this problem, let's use the "divide and conquer" principle:
In order to convert a vector of vectors of points, we need to be able to convert a single vector of points
In order to convert a vector of points, we need to be able to convert a single point
Converting Single Points
This is actually trivial, since cv::Point_ provides a cast operator allowing implicit conversion to points of a different data type. Hence, conversion is done by a simple assignment:
cv::Point2i p1 = { 1,2 };
cv::Point2f p2 = p1;
Converting Vectors of Points
Since implicit conversion of points is possible, this is just as easy -- we simply construct the new vector initializing it using an iterator pair:
pi_vec v1 = { {1,2}, {3,4}, {5,6} };
pf_vec v2{ v1.begin(), v1.end() };
Converting Vectors of Vectors of Points
We do the above in a loop. To improve performance, we can reserve space in the destination vector to avoid reallocations. Then we use emplace_back to construct vectors of points in-place, using an iterator pair to initialize them as above.
pi_vec_vec vv1 = {
{ {1,2}, {3,4}, {5,6} }
, { {7, 8}, {9,10} }
};
pf_vec_vec vv2;
vv2.reserve(vv1.size());
for (auto const& pts : vv1) {
vv2.emplace_back(pts.begin(), pts.end());
}
Additional Operations During Conversion
The answer by #wohlstad already provides some possible approaches. However, looking at the second piece of code using std::transform made me wonder whether there was a way to make it a bit less verbose, perhaps taking advantage of features provided by more recent standard (like c++20). Here is my approach using std::views::transform.
First we create a "range adapter closure" wrapping our conversion lambda function, which applies some constant scaling and offset:
auto const tf = std::views::transform(
[](cv::Point2i const& pt) -> cv::Point2f
{
return cv::Point2f(pt) * 0.5 + cv::Point2f{ 1, -1 };
});
Next, we create an outer transform view of the input vector of vectors of integer points. The lambda function will create an inner transform view using the previously created "range adapter closure", and use this view to construct a vector of float points:
auto const tf_view2 = std::views::transform(vv1
, [&tf](pi_vec const& v) -> pf_vec
{
auto const tf_view = v | tf;
return { tf_view.begin(), tf_view.end() };
});
Finally, we'll construct a vector of vectors of float points, using an iterator pair of this view:
pf_vec_vec vv3{ tf_view2.begin(), tf_view2.end()};
Example Code
#include <opencv2/opencv.hpp>
#include <algorithm>
#include <ranges>
template <typename T>
void dump_v(std::vector<T> const& v)
{
for (auto const& e : v) {
std::cout << e << ' ';
}
std::cout << '\n';
}
template <typename T>
void dump_vv(std::vector<std::vector<T>> const& vv)
{
for (auto const& v : vv) {
dump_v(v);
}
std::cout << '\n';
}
int main()
{
using pi_vec = std::vector<cv::Point2i>;
using pf_vec = std::vector<cv::Point2f>;
using pi_vec_vec = std::vector<pi_vec>;
using pf_vec_vec = std::vector<pf_vec>;
pi_vec_vec vv1 = {
{ {1,2}, {3,4}, {5,6} }
, { {7, 8}, {9,10} }
};
dump_vv(vv1);
pf_vec_vec vv2;
vv2.reserve(vv1.size());
for (auto const& pts : vv1) {
vv2.emplace_back(pts.begin(), pts.end());
}
dump_vv(vv2);
auto const tf = std::views::transform(
[](cv::Point2i const& pt) -> cv::Point2f
{
return cv::Point2f(pt) * 0.5 + cv::Point2f{ 1, -1 };
});
auto const tf_view2 = std::views::transform(vv1
, [&tf](pi_vec const& v) -> pf_vec
{
auto const tf_view = v | tf;
return { tf_view.begin(), tf_view.end() };
});
pf_vec_vec vv3{ tf_view2.begin(), tf_view2.end()};
dump_vv(vv3);
return 0;
}
Example Output
[1, 2] [3, 4] [5, 6]
[7, 8] [9, 10]
[1, 2] [3, 4] [5, 6]
[7, 8] [9, 10]
[1.5, 0] [2.5, 1] [3.5, 2]
[4.5, 3] [5.5, 4]
In Eigen C/C++ Library, how to converter the operation result (example below) from Eigen Matrix to C/C++ Array?
Example:
const Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>( array_C_input , 3, 3);
const Eigen::MatrixSquareRootReturnValue<Eigen::MatrixXf> result = m.sqrt();
float* array_C_output = (float*) result; // Error: convert sqrt output to C array
If you want to compute the matrix root of a matrix passed as C-style array and handle the result like a C-style array, you can either store the result into a MatrixXf and use the data() member of that matrix:
Eigen::MatrixXf matrix_root = Eigen::MatrixXf::Map( array_C_input , 3, 3).sqrt();
float* array_C_output = matrix_root.data();
Alternatively, if you already have memory allocated for the result, you can map the output to that:
void foo(float* output_array, float const* input_array) {
Eigen::MatrixXf::Map( output_array , 3, 3) =
Eigen::MatrixXf::Map( input_array , 3, 3).sqrt();
}
Note that Matrix::sqrt computes the matrix-root, i.e., if S = A.sqrt(), then S*S == M. If you want an element-wise root, you need to use
Eigen::ArrayXXf::Map( input_array , 3, 3).sqrt()
I'm trying to reproduce some numpy code on Gaussian Processes (from here) using Eigen. Basically, I need to sample from a multivariate normal distribution:
samples = np.random.multivariate_normal(mu.ravel(), cov, 1)
The mean vector is currently zero, while the covariance matrix is a square matrix generated via the isotropic squared exponential kernel:
sqdist = np.sum(X1**2, 1).reshape(-1, 1) + np.sum(X2**2, 1) - 2 * np.dot(X1, X2.T)
return sigma_f**2 * np.exp(-0.5 / l**2 * sqdist)
I can generate the covariance matrix just fine for now (it can probably be cleaned but for now it's a POC):
Matrix2D kernel(const Matrix2D & x1, const Matrix2D & x2, double l = 1.0, double sigma = 1.0) {
auto dists = ((- 2.0 * (x1 * x2.transpose())).colwise()
+ x1.rowwise().squaredNorm()).rowwise() +
+ x2.rowwise().squaredNorm().transpose();
return std::pow(sigma, 2) * ((-0.5 / std::pow(l, 2)) * dists).array().exp();
}
However, my problems start when I need to sample the multivariate normal.
I've tried using the solution proposed in this accepted answer; however, the decomposition only works with covariance matrices of size up to 30x30; more than that and LLT fails to decompose the matrix. The alternative version provided in the answer also does not work, and creates NaNs. I tried LDLT as well but it also breaks (D contains negative values, so sqrt gives NaN).
Then, I got curious, and I looked into how numpy does this. Turns out the numpy implementation uses SVD decomposition (with LAPACK), rather than Cholesky. So I tried copying their implementation:
// SVD on the covariance matrix generated via kernel function
Eigen::BDCSVD<Matrix2D> solver(covs, Eigen::ComputeFullV);
normTransform = (-solver.matrixV().transpose()).array().colwise() * solver.singularValues().array().sqrt();
// Generate gaussian samples, "randN" is from the multivariate StackOverflow answer
Matrix2D gaussianSamples = Eigen::MatrixXd::NullaryExpr(1, means.size(), randN);
Eigen::MatrixXd samples = (gaussianSamples * normTransform).rowwise() + means.transpose();
The various minuses are me trying to exactly reproduce numpy's results.
In any case, this works perfectly fine, even with large dimensions. I was wondering why Eigen is not able to do LLT, but SVD works. The covariance matrix I use is the same. Is there something I can do to simply use LLT?
EDIT: Here is my full example:
#include <iostream>
#include <random>
#include <Eigen/Cholesky>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
using Matrix2D = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign>;
using Vector = Eigen::Matrix<double, Eigen::Dynamic, 1>;
/*
We need a functor that can pretend it's const,
but to be a good random number generator
it needs mutable state.
*/
namespace Eigen {
namespace internal {
template<typename Scalar>
struct scalar_normal_dist_op
{
static std::mt19937 rng; // The uniform pseudo-random algorithm
mutable std::normal_distribution<Scalar> norm; // The gaussian combinator
EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
template<typename Index>
inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
};
template<typename Scalar> std::mt19937 scalar_normal_dist_op<Scalar>::rng;
template<typename Scalar>
struct functor_traits<scalar_normal_dist_op<Scalar> >
{ enum { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
} // end namespace internal
} // end namespace Eigen
Matrix2D kernel(const Matrix2D & x1, const Matrix2D & x2, double l = 1.0, double sigma = 1.0) {
auto dists = ((- 2.0 * (x1 * x2.transpose())).colwise() + x1.rowwise().squaredNorm()).rowwise() + x2.rowwise().squaredNorm().transpose();
return std::pow(sigma, 2) * ((-0.5 / std::pow(l, 2)) * dists).array().exp();
}
int main() {
unsigned size = 50;
unsigned seed = 1;
Matrix2D X = Vector::LinSpaced(size, -5.0, 4.8);
Eigen::internal::scalar_normal_dist_op<double> randN; // Gaussian functor
Eigen::internal::scalar_normal_dist_op<double>::rng.seed(seed); // Seed the rng
Vector means = Vector::Zero(X.rows());
auto covs = kernel(X, X);
Eigen::LLT<Matrix2D> cholSolver(covs);
// We can only use the cholesky decomposition if
// the covariance matrix is symmetric, pos-definite.
// But a covariance matrix might be pos-semi-definite.
// In that case, we'll go to an EigenSolver
Eigen::MatrixXd normTransform;
if (cholSolver.info()==Eigen::Success) {
std::cout << "Used LLT\n";
// Use cholesky solver
normTransform = cholSolver.matrixL();
} else {
std::cout << "Broken\n";
Eigen::BDCSVD<Matrix2D> solver(covs, Eigen::ComputeFullV);
normTransform = (-solver.matrixV().transpose()).array().colwise() * solver.singularValues().array().sqrt();
}
Matrix2D gaussianSamples = Eigen::MatrixXd::NullaryExpr(1, means.size(), randN);
Eigen::MatrixXd samples = (gaussianSamples * normTransform).rowwise() + means.transpose();
return 0;
}
I'm a new learner to Ceres Solver, when adding the residualblock using
problem.AddResidualBlock( new ceres::AutoDiffCostFunction<Opt, 1, 6> (new Opt(Pts[i][j].x, Pts[i][j].y, Pts[i][j].z, Ns[i].at<double>(0, 0), Ns[i].at<double>(1, 0), Ns[i].at<double>(2, 0), Ds[i], weights[i]) ),
NULL,
param );
where param is double[6];
struct Opt
{
const double ptX, ptY, ptZ, nsX, nsY, nsZ, ds, w;
Opt( double ptx, double pty, double ptz, double nsx, double nsy, double nsz, double ds1, double w1):
ptX(ptx), ptY(pty), ptZ(ptz), nsX(nsx), nsY(nsy), nsZ(nsz), ds(ds1), w(w1) {}
template<typename T>
bool operator()(const T* const x, T* residual) const
{
Mat R(3, 3, CV_64F), r(1, 3, CV_64F);
Mat inverse(3,3, CV_64F);
T newP[3];
T xyz[3];
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
Rodrigues(r, R);
inverse = R.inv();
newP[0]=T(ptX)-x[3];
newP[1]=T(ptY)-x[4];
newP[2]=T(ptZ)-x[5];
xyz[0]= inverse.at<T>(0, 0)*newP[0] + inverse.at<T>(0, 1)*newP[1] + inverse.at<T>(0, 2)*newP[2];
xyz[1] = inverse.at<T>(1, 0)*newP[0] + inverse.at<T>(1, 1)*newP[1] + inverse.at<T>(1, 2)*newP[2];
xyz[2] = inverse.at<T>(2, 0)*newP[0] + inverse.at<T>(2, 1)*newP[1] + inverse.at<T>(2, 2)*newP[2];
T ds1 = T(nsX) * xyz[0] + T(nsY) * xyz[1] + T(nsZ) * xyz[2];
residual[0] = (ds1 - T(ds)) * T(w);
}
};
but when I output the x[0], I got this:
[-1.40926 ; 1, 0, 0, 0, 0, 0]
after I change the type of the x to double
I got this error :
note: no known conversion for argument 1 from ‘const ceres::Jet<double, 6>* const’ to ‘const double*’
in
bool operator()(const double* const x, double* residual) const
what's wrong with my codes?
Thanks a lot!
I am guessing you are using cv::Mat.
The reason the functor is templated is because Ceres evaluates it using doubles when it needs just the residuals, and evaluates with ceres:Jet objects when it needs to compute the Jacobian. So your attempt to fill r as
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
are trying to convert a Jet into a double. Which is what the compiler is correctly complaining about.
you can re-write your code as (I have not compiled it, so there maybe a minor typo or two).
template<typename T>
bool operator()(const T* const x, T* residual) const {
const T inverse_rotation[3] = {-x[0], -x[1], -x[3]};
const T newP[3] = {ptX - x[3], ptY - x[4]. ptZ - x[5]};
T xyz[3];
ceres::AngleAxisRotatePoint(inverse_rotation, newP, xyz);
const T ds1 = nsX * xyz[0] + nsY * xyz[1] + nsZ * xyz[2];
residual[0] = (ds1 - ds) * w;
return true;
}
The automatic derivatives (AutoDiff) needs a templated cost function to keep track of the operations.
Please take a look at the ceres documentation (http://ceres-solver.org/nnls_modeling.html#autodiffcostfunction). There are a lot of nice examples too. I used them as starting point for my first ceres experiments.
I'm not sure if you can use ceres cost functions with OpenCV functions. In most cases Eigen is used to make the cost function.
Ceres comes with a lot of "ready-to-use" components for cost functions like yours.
I would like to implement a neural network framework consisting of layers which can then be composed into a computational graph (see for example caffe). I am using the eigen library for matrices. Eigen distinguishes between vectors and matrices so that for some operations (adding a bias to a matrix) only a vector can be used (and not a matrix with the same dimensions as the vector). For example:
MatrixXf A = MatrixXf(3, 2); // Variables not initialized for brevity
VectorXf v = VectorXf(2);
MatrixXf R1 = A.array().rowwise() + v.transpose().array(); // Broadcasts v correctly
MatrixXf vMat = MatrixXf(1, 2);
MatrixXf R2 = A.array().rowwise() + vMat.array(); // YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX Error
If I want the layers to look something like this:
void AffineForward(std::vector<Tensor> in, std::vector<Tensor> out)
{
MatrixXf &X = in[0];
MatrixXf &W = in[1];
VectorXf &b = in[2];
out[0] = X * W;
out[0] += b;
}
how would I design the abstract Tensor class so that I can just send in a std::vector of Tensors? I thought about something like this:
class Tensor
{
public:
virtual Tensor operator*(const Tensor &t) const = 0;
};
class TensorMatrix : Tensor
{
public:
TensorMatrix operator*(const TensorMatrix &t) const;
TensorMatrix operator*(const TensorVector &t) const;
MatrixXf _data;
};
class TensorVector : Tensor
{
public:
VectorXf _data;
};
but the virtual Tensor operator* throws a compile time error (function returning abstract class Tensor is not allowed) which makes sense.
What is the easiest way of doing what I want? Creating some class that could be put into a container and I could get both MatrixXf and VectorXf out of it (depending on what the user put in?). Caffe uses something called 'Blob'.
Eigen distinguishes between vectors and matrices so that for some operations (adding a bias to a matrix) only a vector can be used (and not a matrix with the same dimensions as the vector).
this is not true, a vector is a matrix in Eigen; it's just that some operations require dimensions to be known at compile time; in your example
MatrixXf R1 = A.rowwise() + v.transpose();
MatrixXf R2 = A.rowwise() + vMat;
the second line does not compile because that broadcasting needs a matrix with a compile time row-dimensions == 1;
the solution is to tell Eigen you want a row vector explictly:
MatrixXf R2 = A.rowwise() + vMat.row(0);
a code working with both row and column vectors stored as MatrixXf being something like ( whether this advisable or not depending on your ultimate requirements )
if( vMat.rows() == 1 )
MatrixXf R1 = A.rowwise() + vMat.rows(0); ...
else if( vMat.cols() == 1 )
MatrixXf R2 = A.rowwise() + vMat.transpose().rows(0); ...
else
whatever...
so, you can always store vectors as matrices with Eigen, you just need some care in telling Eigen what to do with them ...