I am trying to achieve solution for a Multilateration problem. The input data i have are Global pose of the landmarks and distances from it. My costfunction is following
struct CostFunctor {
// Initializing constant parameters
CostFunctor(std::vector<double>& a_n, float d_n)
: a_n_(a_n), d_n_(d_n) {}
/*
Residual
\f$f_n(x) = ((a_{n_1} - x_1)^2 + (a_{n_2} - x_2)^2 + (a_{n_3} - x_3)^2 - d_n^2)\f$
*/
template <typename T>
bool operator()(const T* const x,
T* residual) const {
residual[0] = ((T(a_n_[0]) - x[0]) * (T(a_n_[0]) - x[0])
+ (T(a_n_[1]) - x[1]) * (T(a_n_[1]) - x[1])
+ (T(a_n_[2]) - x[2]) * (T(a_n_[2]) - x[2])
- (T(d_n_) * T(d_n_))
);
return true;
}
private:
const std::vector<double>& a_n_;
const float d_n_;
};
And the main code is
std::vector<double> solveProblem(std::vector<double> &position, std::vector<std::vector<double>> &global_pose, std::vector<float> &dists)
{
std::vector<double> x = {0.0, 0.0, 0.0};
problem.AddParameterBlock(x.data(), 3);
//ceres::Lossfunction *loss_function = NULL;
for (unsigned int i = 0; i < dists.size(); i++) {
ceres::CostFunction *cost_function =
new ceres::AutoDiffCostFunction<CostFunctor, 1, 3>(
new CostFunctor(global_pose[i], dists[i]));
std::cout << "in solve problem for" << std::endl;
problem.AddResidualBlock(cost_function,
NULL,
x.data());
}
// Run the solver
options.max_num_iterations = 50;
options.minimizer_progress_to_stdout = true;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
double* dPos = x.data();
for (unsigned int i = 0; i < 3; i++) {
position[i] = *dPos++;
}
return position;
}
However, i get the segmentation fault error. Edit I run this file as a ROSnode, which subscribes to a topic to get the input data (i.e global_pose, dists) The solver runs first time successfully and then throws segmentation error.
Parth
