When I run the following code in tutorial of ceres solver,I met some problem.
Here is the code:
#include <iostream>
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
int main(int argc, char *argv[])
{
double x = 0.5;
const double initial_x = x;
Problem problem;
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x << " -> " << x << "\n";
}
And I got the problem:
E:\study_materials\Computer Version\ceres\ceres-solver-2.0.0\internal\ceres\solver.cc:505 Terminating: Can't use SPARSE_NORMAL_CHOLESKY with Solver::Options::sparse_linear_algebra_library_type = SUITE_SPARSE, because support was not enabled when Ceres Solver was built.
I don't know how to enable this support,any assistance will be appreciated.
what version of ceres solver is this? if a sparse linear algebra library is not available, it should default to using a dense solver (DENSE_QR). So this looks like a bug in the way defaults are setup in ceres solver.
Related
I try to provide a kind of "map" skeleton that wraps OneAPI calls hiding hardware targeting issues through some parameter specifying the kind of target (CPU or GPU/Accelerator). my Map skeleton pass function and its derivative with initial point to the Newton method.
but I have an error which is:
kernel parameter has non-trivially copy constructible class/struct type 'std::function<double (double)>'
and my code is:
#include <CL/sycl.hpp>
#include <iostream>
#include <tbb/tbb.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <vector>
#include <string>
#include <queue>
#include<tbb/blocked_range.h>
#include <tbb/global_control.h>
#include <chrono>
#include "uTimer.cpp"
#include <cmath>
#include <random>
#include <ctime>
#include <numeric>
#include <cstdlib>
//#include <dos.h> //for delay
//#include <conio.h> //for getch()
//#include <complex>
#define EPSILON 0.000001 // The step size across the X and Y axis
using namespace tbb;
class Clock {
private:
typedef std::chrono::high_resolution_clock clock;
std::chrono::time_point<clock> t;
public:
Clock() {
start();
}
void start() {
t = clock::now();
}
double stop() const {
return std::chrono::duration_cast<std::chrono::duration<double>>(
clock::now() - t).count();
}
};
//std::complex<double> mycomplex(10.0, 2.0);
template<class Tin, class Tout>
class Map {
private:
std::function<Tout(Tin)> fun;
std::function<Tout(Tin)> dfun;
public:
Map() {};
Map(std::function<Tout(Tin)> f, std::function<Tout(Tin)> df) {
fun = f;
dfun = df;
};
void operator()(bool use_tbb, Tin &x1) {
int iter=100;
Tout x;
if (use_tbb) {
uTimer *timer = new uTimer("Executing Code On CPU");
tbb::parallel_for(tbb::blocked_range < int > (0, iter),
[&](tbb::blocked_range<int> t) {
for (int index = t.begin(); index < t.end(); ++index) {
do
{
x = x1;
x1 = x - (fun(x) / dfun(x));
}while (std::abs(x1 - x) >= EPSILON);
}
});
timer->~uTimer();
}else {
sycl::buffer<Tin, 1> x1_buffer(&x1, iter);
sycl::buffer<Tout, 1> x_buffer(&x, iter);
//Profiling GPU
// Initialize property list with profiling information
sycl::property_list propList {
sycl::property::queue::enable_profiling() };
// Build the command queue (constructed to handle event profling)
sycl::queue gpuQueue = cl::sycl::queue(sycl::gpu_selector(),
propList);
// print out the device information used for the kernel code
std::cout << "Device: "
<< gpuQueue.get_device().get_info<sycl::info::device::name>()
<< std::endl;
std::cout << "Compute Units: "
<< gpuQueue.get_device().get_info<
sycl::info::device::max_compute_units>()
<< std::endl;
auto start_overall = std::chrono::system_clock::now();
auto event = gpuQueue.submit([&](sycl::handler &h) {
//local copy of fun
auto f = fun;
auto df = dfun;
sycl::accessor x1_accessor(x1_buffer, h, sycl::read_write);
sycl::accessor x_accessor(x_buffer, h, sycl::read_write);
h.parallel_for(iter, [=](sycl::id<1> index) {
do
{
x_accessor[index] = x1_accessor[index];
x1_accessor[index] = x_accessor[index] - (f(x_accessor[index]) / df(x_accessor[index]));
}while (sycl::fabs(f(x1_accessor[index]))>= EPSILON);
});
});
event.wait();
auto end_overall = std::chrono::system_clock::now();
cl_ulong submit_time = event.template get_profiling_info<
cl::sycl::info::event_profiling::command_submit>();
cl_ulong start_time = event.template get_profiling_info<
cl::sycl::info::event_profiling::command_start>();
cl_ulong end_time = event.template get_profiling_info<
cl::sycl::info::event_profiling::command_end>();
auto submission_time = (start_time - submit_time) / 1000000.0f;
std::cout << "Submit Time: " << submission_time << " ms"
<< std::endl;
auto execution_time = (end_time - start_time) / 1000000.0f;
std::cout << "Execution Time: " << execution_time << " ms"
<< std::endl;
auto execution_overall = std::chrono::duration_cast
< std::chrono::milliseconds > (end_overall - start_overall);
std::cout << "Overall Execution Time: " << execution_overall.count()
<< " ms" << std::endl;
};
};
};
int main(int argc, char *argv[]) {
//Define a function
auto f = [](double x) {return pow(x,3);};
//Define the derivative of function
auto df = [](double x) {return pow(x, 2) *3;};
//Define an instance of Map class
auto m1 = Map<double, double>(f, df);
double x1 = 3;
m1(true, x1);
//print the result
//for (auto &e : r) {
//std::cout << e << " ";
//}
return 0;
}
In addition, if we do not consider to an error, I think something in my code seems is not correct but I cannot not understand what it is.
You cannot do what you want. If you tried getting rid of std::function and using function pointers you still wouldn't be able to (even if it would be trivially copyable). In SYCL as in any other such language (CUDA, hip, OpenCL,...), the device compiler needs to be able to compile all the functions executed/called by kernel. So no, you cannot pass a function "in". It boils down to one of your previous questions answered here
You could try defining your lambdas as functions somewhere else and then calling them from your kernel. If you want to be able to choose at runtime between various functions, you could write a templated kernel (with let's say an enum) and dispatch your call through an if constexpr (in the kernel) to avoid runtime costs (and code deduplication). At the end that would instantiate n SYCL kernels, each calling one of your functions. They would be properly compiled by the device compiler, etc.
I am currently porting an algorithm from boost::ublas to Eigen:
Code 1 with boost::ublas
#ifndef KHACH_H
#define KHACH_H
#include <set>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
//namespace Minim {
namespace ublas=boost::numeric::ublas;
template<class T>
bool InvertMatrix(const ublas::matrix<T> &input,
ublas::matrix<T> &inverse)
{
using namespace boost::numeric::ublas;
typedef permutation_matrix<std::size_t> pmatrix;
matrix<T> A(input);
pmatrix pm(A.size1());
int res = lu_factorize(A,pm);
if( res != 0 ) return false;
inverse.assign(ublas::identity_matrix<T>(A.size1()));
lu_substitute(A, pm, inverse);
return true;
}
inline void Lift(const ublas::matrix<double> &A,
ublas::matrix<double> &Ap)
{
Ap.resize(A.size1()+1,
A.size2());
ublas::matrix_range<ublas::matrix<double> >
sub(Ap,
ublas::range(0, A.size1()),
ublas::range(0, A.size2()));
sub.assign(A);
ublas::row(Ap, Ap.size1()-1)=ublas::scalar_vector<double>(A.size2(),1.0);
}
#endif
//}
Code 2 with Eigen:
#ifndef KHACH_H
#define KHACH_H
#include <set>
#include <iostream>
#include <Eigen/Eigen>
//namespace Minim {
template <class NT>
using MT = Eigen::Matrix<NT, Eigen::Dynamic, Eigen::Dynamic>;
template <class NT>
using VT = Eigen::Matrix<NT, Eigen::Dynamic, 1>;
template<typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x)
{
return ((x.array() == x.array())).all();
}
template<class T>
bool InvertMatrix(const MT<T> &input,
MT<T> &inverse)
{
inverse.setIdentity(input.rows(), input.cols());
inverse = input.inverse();
return !is_nan(inverse);
}
inline void Lift(const MT<double> &A, MT<double> &Ap)
{
Ap.resize(A.rows()+1, A.cols());
Ap.topLeftCorner(A.rows(), A.cols()) = A;
Ap.row(Ap.rows()-1).setConstant(1.0);
}
#endif
//}
These functions are part of the bigger code and functionality, but I think these two functions are the ones creating the difference. The functions with Eigen are giving a different output for some large matrices compared to the output of the code using boost, I am not able to understand the bugs.
Any help would be appreciated.
You didn't specify any inputs or what the discrepancy is you're finding.
This lead me to build simple testers, in which I find that an obvious source of "differences" is the inaccuracy of [binary] floating point representations.
You can easily confirm it with some test input: whose inverse is :
Live On Compuler Explorer
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <set>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
namespace Minim1 {
namespace ublas = boost::numeric::ublas;
template <class T> using MT = ublas::matrix<T>;
template <class T> bool InvertMatrix(const MT<T>& input, MT<T>& inverse)
{
using namespace boost::numeric::ublas;
typedef permutation_matrix<std::size_t> pmatrix;
matrix<T> A(input);
pmatrix pm(A.size1());
int res = lu_factorize(A, pm);
if (res != 0)
return false;
inverse.assign(ublas::identity_matrix<T>(A.size1()));
lu_substitute(A, pm, inverse);
return true;
}
template <class T>
inline void Lift(const ublas::matrix<T>& A, ublas::matrix<T>& Ap)
{
Ap.resize(A.size1() + 1, A.size2());
ublas::matrix_range<ublas::matrix<T>> sub(
Ap, ublas::range(0, A.size1()), ublas::range(0, A.size2()));
sub.assign(A);
ublas::row(Ap, Ap.size1() - 1) = ublas::scalar_vector<T>(A.size2(), 1.0);
}
}
#include <Eigen/Eigen>
#include <iostream>
#include <set>
namespace Minim2 {
template <class T>
using MT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
static_assert(Eigen::RowMajor == 1);
template <class T>
using VT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::RowMajor>;
template <typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x)
{
return ((x.array() == x.array())).all();
}
template <class T> bool InvertMatrix(const MT<T>& input, MT<T>& inverse)
{
inverse.setIdentity(input.rows(), input.cols());
inverse = input.inverse();
return !is_nan(inverse);
}
template <typename T>
inline void Lift(const MT<T>& A, MT<T>& Ap)
{
Ap.resize(A.rows() + 1, A.cols());
Ap.topLeftCorner(A.rows(), A.cols()) = A;
Ap.row(Ap.rows() - 1).setConstant(1.0);
}
}
template <typename T>
static inline std::string compare(Minim1::MT<T> const& a, Minim2::MT<T> const& b) {
if (a.size1() != static_cast<size_t>(b.rows())) return "rows do not match";
if (a.size2() != static_cast<size_t>(b.cols())) return "cols do not match";
for (size_t r = 0; r < a.size1(); r++) {
for (size_t c = 0; c < a.size2(); c++) {
auto va = a(r,c);
auto vb = b(r,c);
auto delta = std::abs(va-vb);
if (va != vb) {
std::ostringstream oss;
oss
<< "mismatch at (" << r << ", " << c << "): "
<< va << " != " << vb
<< " delta:" << std::abs(va-vb)
<< " significant? " << std::boolalpha
<< (std::numeric_limits<T>::epsilon() < delta) << "\n";
return oss.str();
}
}
}
return "equivalent";
}
template <typename T>
auto convert(Minim1::MT<T> const& a) {
Minim2::MT<T> b(a.size1(), a.size2());
for (size_t r = 0; r < a.size1(); r++) {
for (size_t c = 0; c < a.size2(); c++) {
b(r, c) = a(r, c);
} }
return b;
}
int main() {
using T = double;
using M1 = Minim1::MT<T>;
using M2 = Minim2::MT<T>;
auto report = [](auto const& a, auto const& b) {
std::cout << "\na: ------\n" << a;
std::cout << "\nb: ------\n" << b;
std::cout << "\n" << compare(a, b) << "\n";
};
M1 a(3, 3);
a(0, 0) = 1; a(0, 1) = 2; a(0, 2) = 3;
a(1, 0) = 3; a(1, 1) = 2; a(1, 2) = 1;
a(2, 0) = 2; a(2, 1) = 1; a(2, 2) = 3;
M2 b(3, 3);
b << 1, 2, 3,
3, 2, 1,
2, 1, 3;
report(a, b);
std::cout << "\nINVERSIONS";
M1 ai(a.size1(), a.size2());
M2 bi(b.rows(), b.cols());
Minim1::InvertMatrix(a, ai);
Minim2::InvertMatrix(b, bi);
report(ai, bi);
M2 deltas = (convert(ai) - bi).cwiseAbs();
constexpr auto eps = std::numeric_limits<T>::epsilon();
std::cout << "deltas:\n" << deltas << "\n";
for (int r = 0; r < deltas.rows(); r++) {
for (int c = 0; c < deltas.cols(); c++) {
auto d = deltas(r,c);
if (d > eps) {
std::cout << "The delta at (" << r << ", " << c << ") (" << d << " is > ε (" << eps << ")\n";
}
} }
}
Prints
a: ------
[3,3]((1,2,3),(3,2,1),(2,1,3))
b: ------
1 2 3
3 2 1
2 1 3
equivalent
INVERSIONS
a: ------
[3,3]((-0.416667,0.25,0.333333),(0.583333,0.25,-0.666667),(0.0833333,-0.25,0.333333))
b: ------
-0.416667 0.25 0.333333
0.583333 0.25 -0.666667
0.0833333 -0.25 0.333333
mismatch at (0, 0): -0.416667 != -0.416667 delta:5.55112e-17 significant? false
deltas:
5.55112e-17 0 0
0 2.77556e-17 0
0 2.77556e-17 0
Confirming that all differences are around (even <) the machine epsilon for the chosen data type. If you replace that one:
using T = long double;
You get the following deltas: Compiler Explorer
mismatch at (0, 0): -0.416667 != -0.416667 delta:2.71051e-20 significant? false
deltas:
2.71051e-20 1.35525e-20 0
5.42101e-20 0 0
6.77626e-21 0 0
Where To Go From Here
Find out whether this is your problem by plugging in your inputs. You might stumble on other things that escaped your attention before. If not, at least you now have the tools to make a new, more focused question.
If you want to learn more about floating point inaccuracy:
Why are floating point numbers inaccurate?
Is floating point math broken?
How can I pass some constants in boost::math::tools::brent_find_minima() from main()?
struct func
{
template <class T>
T operator()(T const& x)
{ //
T Wnew = 20.0/9.0*720.0; // Goal is to pass it through main()
T W = 2500; // Goal is to pass it through main()
return abs(Wnew/2/x - atan(W/2/x));
}
};
int main(int argc, char **argv)
{
// How can I pass Wnew and W values while calling boost::math::tools::brent_find_minima() from main()
std::pair<double, double> r = boost::math::tools::brent_find_minima(func(), 1.0, 2000.0, std::numeric_limits<double>::digits);
std::cout.precision(std::numeric_limits<double>::digits10);
std::cout << "x at minimum = " << r.first << ", f(" << r.first << ") = " << r.second << std::endl;
}
I suppose what you want is to create different instances of func with different values for W and Wnew. You are almost there, just give your func some state:
struct func
{
double Wnew;
double W;
func(double Wnew, double W) : Wnew(Wnew),W(W) {}
double operator()(double const& x)
{
return abs(Wnew/2/x - atan(W/2/x));
}
};
And then create an instance like this:
double Wnew = 1.0;
double W = 2.0;
auto r = boost::math::tools::brent_find_minima(func(Wnew,W), 1.0, 2000.0, std::numeric_limits<double>::digits);
// ^^
I was a bit puzzled by your operator() being a template and changed that. If you had good reasons for it, simply make it a template again.
PS: Since C++11, there are lambda expressions that allow a much terser syntax for functors.
I like the lambda approach the best:
#include <cmath>
#include <iostream>
#include <boost/math/tools/minima.hpp>
int main(int argc, char **argv)
{
double W = 2500;
double Wnew = 20.0/9.0*720.0;
if (argc == 3) {
double W = std::stod(argv[1]);
double Wnew = std::stod(argv[2]);
}
auto f = [&W, &Wnew](double x)->double {
return std::abs(Wnew/2/x - atan(W/2/x));
};
std::pair<double, double> r = boost::math::tools::brent_find_minima(f, 1.0, 2000.0, std::numeric_limits<double>::digits);
std::cout.precision(std::numeric_limits<double>::digits10);
std::cout << "x at minimum = " << r.first << ", f(" << r.first << ") = " << r.second << std::endl;
}
The documentation for brent_find_minima was written pre-C++11, and uses what now should be regarded as overly verbose syntax. The post C++11 boost.math documentation examples (like the quadrature routines) all use lambdas.
I want to use Ceres to optimise a function of n parameters. How to find the gradient of this function is unclear, although I do have a well defined cost. I have been using GSL so far, with numeric differentiation, but I thought I would try to use Ceres with automatic diff.
I have had a look at the toy example helloworld_analytic_diff.cc in which they use AutoDiff to minimise the function f(x) = 0.5 (10 - x)^2 and read the tutorial, so I thought I would try and extend this to the two dimensional function f(x,y) = (10-x)^2 +(20- y)^2, which has a global minimum at x, y = 10, 20. But I am getting a little stuck on this:
#include "ceres/ceres.h"
#include "glog/logging.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
struct CostFunctor {
template <typename T> bool operator()(const T* const x, T* residual) const {
const T x1 = x[0];
const T y1 = x[1];
residual[0] = (10.0-x[0]) + (20.0-x[1]);
return true;
}
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
double x[2] = {0.5, -3.0};
const double initial_x[2] = {0.5, -3.0};
Problem problem;
CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 2>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x[0]);
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x[0] << ", " << initial_x[0]
<< " -> " << x[0] << ", " << x[1]<< "\n";
return 0;
}
If I run this however, it winds up converging to something incorrect, depending on the initial guess:
Ceres Solver Report: Iterations: 3, Initial cost: 5.281250e+02, Final cost: 3.667046e-16, Termination: CONVERGENCE
x : 0.5, 0.5 -> 16.75, 13.25
Any ideas on what I've done wrong here?
Many thanks!
Your cost functor is wrong.
the optimization problem you are solving is
[(10.0-x) + (20.0-y)]^2
and not
(10-x)^2 +(20- y)^2,
what you should do here is to have two cost functors, something like as follows:
struct CostFunctor1 {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0-x[0];
return true;
}
};
struct CostFunctor2 {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = 20.0-x[1];
return true;
}
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
double x[2] = {0.5, -3.0};
const double initial_x[2] = {0.5, -3.0};
Problem problem;
problem.AddResidualBlock(new AutoDiffCostFunction<CostFunctor1, 1, 2>(new
CostFunctor1), NULL, &x[0]);
problem.AddResidualBlock(new AutoDiffCostFunction<CostFunctor2, 1, 2>(new
CostFunctor2), NULL, &x[0]);
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x[0] << ", " << initial_x[0]
<< " -> " << x[0] << ", " << x[1]<< "\n";
return 0;
}
I am trying to use ceres solver to optimize the point cloud transformation process.
Through following the samples from ceres solver tutorial, I got a simple working version of the optimization process. However, when I try to further modify the function in operator (in MyCostFunctor class), the results are totally wrong (the solver converges, but gives wrong results). I found the problem is caused by the two lines of codes where I was trying to convert the parameters from template type T to Eigen matrix type.
Here are the codes:
template<typename T> inline
void DataTransfer(const T* input, Eigen::Matrix<T, Eigen::Dynamic, 1>& output) {
for (int i = 0; i < 12; ++i) {
output[i] = input[i];
}
}
template<typename T, typename PtT> inline
T* GetCorrespondingPoint(const T* rot, const PtT pt) {
//**!!!!!!!!!!! Error !!!!!!!!!!!**
//Eigen::Matrix<T, Eigen::Dynamic, 1> param_vecs = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(12);
//DataTransfer<T>(rot, param_vecs);
// **!!!!!!!!!! Error !!!!!!!!!!!**
T result[3];
result[0] = rot[0] * T(pt(0)) + rot[1] * T(pt(1)) + rot[2] * T(pt(2)) + rot[9];
result[1] = rot[3] * T(pt(0)) + rot[4] * T(pt(1)) + rot[5] * T(pt(2)) + rot[10];
result[2] = rot[6] * T(pt(0)) + rot[7] * T(pt(1)) + rot[8] * T(pt(2)) + rot[11];
return result;
}
// A cost functor that implements the residual r = x - y.
// where x = R*x' + T or add more operations such as x = C*inverse((R*x')*A + T*B), A, B, C are related vectors or matrices
template<typename PtT>
class MyCostFunctor {
public:
MyCostFunctor(PtT& x, PtT& y, int pt_id)
:x_(x), y_(y), idx_(pt_id) {
}
template<typename T>
bool operator()(const T* const params, T* residual) const {
// Data transformation
T* rslt;
rslt = GetCorrespondingPoint<T, PtT>(params, x_);
residual[0] = T(rslt[0] - y_(0));
residual[1] = T(rslt[1] - y_(1));
residual[2] = T(rslt[2] - y_(2));
return true;
}
private:
PtT x_; // source point
PtT y_; // target point
int idx_; // source point idx
};
The two lines of codes are commented out in function "GetCorrespondingPoint".
The code of the main function are as follows:
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include "ceres/ceres.h"
#include "glog/logging.h"
#include "ceres/dynamic_autodiff_cost_function.h"
using ceres::NumericDiffCostFunction;
using ceres::AutoDiffCostFunction;
using ceres::SizedCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
int main(int argc, char** argv){
google::InitGoogleLogging(argv[0]);
// 1. Sample Data Set Up
std::vector<Eigen::Vector3d> model_pts;
model_pts.clear();
std::vector<Eigen::Vector3d> target_pts;
target_pts.clear();
model_pts.push_back(Eigen::Vector3d(10.0, 10.0, 10.0));
model_pts.push_back(Eigen::Vector3d(20.0, 10.0, 10.0));
model_pts.push_back(Eigen::Vector3d(10.0, 20.0, 10.0));
model_pts.push_back(Eigen::Vector3d(10.0, 10.0, 20.0));
target_pts.push_back(Eigen::Vector3d(40.0, 40.0, 40.0));
target_pts.push_back(Eigen::Vector3d(40.0, 30.0, 40.0));
target_pts.push_back(Eigen::Vector3d(30.0, 40.0, 40.0));
target_pts.push_back(Eigen::Vector3d(40.0, 40.0, 30.0));
/// Set up the index for pairing the model and target points
std::vector<int> pt_idx;
pt_idx.push_back(0);
pt_idx.push_back(1);
pt_idx.push_back(2);
pt_idx.push_back(3);
// print pts
std::cout << "Model pts\t\tTarget pts\n";
for (int i = 0; i < model_pts.size(); ++i) {
std::cout << model_pts[i](0) << " " << model_pts[i](1) << " " << model_pts[i](2) << "\t\t\t"
<< target_pts[i](0) << " " << target_pts[i](1) << " " << target_pts[i](2) << "\n";
}
// Parameter Set up
double params[12];
for (int i = 0; i < 12; ++i) {
params[i] = 1.0;
}
// Set up the problem
int num_pts = target_pts.size();
Problem problem;
for (int i = 0; i < num_pts; ++i) {
problem.AddResidualBlock(
new AutoDiffCostFunction<MyCostFunctor<Eigen::Vector3d>, 3, 12>(new MyCostFunctor<Eigen::Vector3d>(model_pts[i], target_pts[i], pt_idx[i])), NULL,¶ms[0]);
}
// Set the solver options
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
// Run the solver!
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n\n";
// print results
std::cout << "test results: \n";
for (int i = 0; i < model_pts.size(); ++i) {
Eigen::Vector3d pt;
pt(0) = params[0]*model_pts[i](0) + params[1]*model_pts[i](1) + params[2]*model_pts[i](2) + params[9];
pt(1) = params[3]*model_pts[i](0) + params[4]*model_pts[i](1) + params[5]*model_pts[i](2) + params[10];
pt(2) = params[6]*model_pts[i](0) + params[7]*model_pts[i](1) + params[8]*model_pts[i](2) + params[11];
std::cout << pt(0) << " " << pt(1) << " " << pt(2) << "\n";
}
return 0;
}
If I comment out the two lines, I will get right results:
results before data transfer
However, when I am trying to transfer the parameters into Eigen formate with those two lines of codes (NOT BEEN USED in the function, only copy and transfer), I will get the wrong results:
results after data transfer
Can anyone help me to figure out what's the problem and what should I do if I want to do some operation on the parameters to get the right corresponding points? Thanks!
Your code for the residual uses the matrix rot in row-major form, while Eigen defaults to column-major form:
https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html