1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
| #include <ceres/ceres.h>
std::vector<std::array<double,4>> data = {
{{ 1, 2, 10, 146}},
{{ 7, -2, 0, 66}},
{{14, 4, -2, 49}},
{{ 2, 5, 8, 86}}
};
struct MyResidual {
const double x_, y_, z_, d2_;
MyResidual(double x, double y, double z, double d2) : x_(x), y_(y), z_(z), d2_(d2) {}
template <typename T>
bool operator()(const T* const x, const T* const y, const T* const z, T* residual) const {
T dx = *x - T(x_);
T dy = *y - T(y_);
T dz = *z - T(z_);
T d2 = T(d2_);
residual[0] = d2 - dx*dx - dy*dy - dz*dz;
return true;
}
};
int main() {
double x = 0.0, y = 0.0, z = 0.0;
ceres::Problem problem;
for (const auto & a : data) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<MyResidual, 1, 1, 1, 1>(
new MyResidual(a[0], a[1], a[2], a[3])),
NULL, &x, &y, &z);
}
ceres::Solver::Options options;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << std::endl;
std::cout << "Final: " << x << " " << y << " " << z << std::endl;
return 0;
} |
Partager