# Seeking help with my can locator problem

34 views

### max

Jun 22, 2023, 8:42:04 PMJun 22
to Ceres Solver
// I have a problem.
// The kid next door (lets call him Sheldon) saw me drink some of my dads beers.
// Seven to be exact... Voodoo Rangers.  I threw the cans into his back yard and
// they are spread all over his yard.
// He threatened to tell my dad if I can't compute their location in 3d coordinate space...
// By just measuring their distance between the cans!  I used a tape measure.
// I picked one beer can to be the home base.  I measured the distance between a few
// cans but I could not get every measurement. Some of the measurements weren't too good.
// The cans are all close to the ground so it should be easy, right?
// I googled the google problem solver and I came up with this and it seems to work.
// But Sheldon looked at my computer screen and saw the SQRT function. He laughed and
// ran back to his house, saying "Jacobian of a square root hahaha you idiot".
// Now I'm afraid I did something wrong...  Is there a way to fix it?
// I've included my program.
// My name is Max., thank you for your help.

#include <iostream>
#include <ceres/ceres.h>
#include <vector>

// A thing to hold the location of a beer can
struct Location {
double x;
double y;
double z;
};

// A thing to hold the canA to canB distance measurements
struct Measurement {
unsigned int canA;  // the first can number
unsigned int canB;  // the second can number
double distance;  // the measured distance between canA and canB

// google says I can weight the measurements with the inverse of the variance.
//double variance;  // the error in the distance measurement how do I use this?
};

// The magic problem solver calls this a bunch of times to "hone in"
// I think it works by taking a guess and then this returns how wrong it is.
struct ErrorFunctor {

ErrorFunctor(double measurement)
: measurement_(measurement) {}

template<typename T>
bool operator()(const T* const x1, const T* const y1, const T* const z1,
const T* const x2, const T* const y2, const T* const z2,
T* residual) const
{
// this sqrt is what Sheldon is laughing about...
residual[0] = measurement_ - sqrt((x1[0]-x2[0])*(x1[0]-x2[0]) + (y1[0]-y2[0])*(y1[0]-y2[0]) + (z1[0]-z2[0])*(z1[0]-z2[0]));

return true;
}

const double measurement_;
};

int main(int argc, char** argv)
{

// A thing to hold all my distance measurements
std::vector<Measurement> distances = {
{0, 1, 10.03},
{0, 2, 14.13},
{1, 2, 10.01},
{0, 3, 10.06},
{1, 3, 14.10},
{2, 3, 9.95},
{0, 4, 4.99},
{1, 4, 11.25},
{2, 4, 11.23},
{3, 4, 5.02},
{0, 5, 11.20},
{1, 5, 11.27},
{2, 5, 4.97},
{3, 5, 5.05},
{0, 6, 19.95},
{2, 6, 14.14},
{4, 6, 15.05},
{3, 6,  9.95}
};

// A thing to hold all the beer can locations
std::vector<Location> locations(7);

// Initialize the x,y,z positions of the mostly known cans
locations[0] = { 0.0,  0.0, 1.0};
locations[1] = {10.0,  0.0, 1.0};
locations[2] = {10.0, 10.0, 1.0};

// Initialize the mostly unknown cans -- wild guesses!
locations[3] = {0.1,   0.2,  1.0};  // failure if you start with zeros??
locations[4] = {10,     5,   1.0};
locations[5] = {-2,     4,   1.0};
locations[6] = { 0.1,   25,  1.0};

// this is the magic problem solver
ceres::Problem problem;

// First I tell the magic problem solver where I think the cans might be
// Some locations I'm fairly sure about but others not at all.
// So I add constant parameter blocks (constraints) for the things I'm sure about.
problem.SetParameterBlockConstant(&locations[0].x);
problem.SetParameterBlockConstant(&locations[0].y);
problem.SetParameterBlockConstant(&locations[0].z);

problem.SetParameterBlockConstant(&locations[1].x);
//   problem.SetParameterBlockConstant(&locations[1].y);
problem.SetParameterBlockConstant(&locations[1].z);

//  problem.SetParameterBlockConstant(&locations[2].x);
problem.SetParameterBlockConstant(&locations[2].y);
problem.SetParameterBlockConstant(&locations[2].z);

// Add parameter blocks for the unknown cans
//  problem.SetParameterBlockConstant(&locations[3].z);

//  problem.SetParameterBlockConstant(&locations[4].z);

//  problem.SetParameterBlockConstant(&locations[5].z);

//  problem.SetParameterBlockConstant(&locations[6].z);

// This tells the magic problem solver about all my measurements.
for (const auto& measure : distances) {
unsigned int canA   = measure.canA;
unsigned int canB   = measure.canB;
double distAB = measure.distance;

new ceres::AutoDiffCostFunction<ErrorFunctor, 1, 1, 1, 1, 1, 1, 1>(
new ErrorFunctor(distAB)),
nullptr,
&locations[canA].x, &locations[canA].y, &locations[canA].z,
&locations[canB].x, &locations[canB].y, &locations[canB].z
);
}

ceres::Solver::Options options;
options.max_num_iterations = 100;
//options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;

// This is the where the magic happens!
ceres::Solve(options, &problem, &summary);

std::cout << summary.FullReport() << std::endl;

std::cout << "Estimated Beer Can Locations:" << std::endl;
for (unsigned int i = 0; i < locations.size(); ++i) {
std::cout << "Can " << i << ": (" << locations[i].x << ", " << locations[i].y << ", " << locations[i].z << ")" << std::endl;
}

return 0;
}

### max

Jun 22, 2023, 8:43:00 PMJun 22
to Ceres Solver
This is the output of my program:

0  3.573626e+02    0.00e+00    2.96e+01   0.00e+00   0.00e+00  1.00e+04        0    4.16e-04    5.36e-04
1  1.193740e+02    2.38e+02    2.01e+01   1.87e+01   7.44e-01  1.13e+04        1    4.47e-04    1.05e-03
2  2.110225e+00    1.17e+02    2.12e+00   8.28e+00   1.02e+00  3.39e+04        1    3.54e-04    1.42e-03
3  1.401857e-02    2.10e+00    1.51e-01   1.72e+00   1.01e+00  1.02e+05        1    3.23e-04    1.76e-03
4  7.353037e-03    6.67e-03    4.07e-04   9.16e-02   1.00e+00  3.05e+05        1    3.59e-04    2.14e-03
5  7.352950e-03    8.67e-08    2.50e-06   3.08e-04   1.00e+00  9.16e+05        1    3.49e-04    2.50e-03

Solver Summary (v 1.14.0-eigen-(3.3.7)-lapack-suitesparse-(5.7.1)-cxsparse-(3.2.0)-eigensparse-openmp-no_tbb)

Original                  Reduced
Parameter blocks                           21                       14
Parameters                                 21                       14
Residual blocks                            18                       18
Residuals                                  18                       18

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Linear solver ordering              AUTOMATIC                       14

Cost:
Initial                          3.573626e+02
Final                            7.352950e-03
Change                           3.573553e+02

Minimizer iterations                        6
Successful steps                            6
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000120

Residual only evaluation           0.000031 (6)
Jacobian & residual evaluation     0.001961 (6)
Linear solver                      0.000224 (6)
Minimizer                            0.002431

Postprocessor                        0.000002
Total                                0.002554

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 4.321291e-10 <= 1.000000e-06)

Estimated Beer Can Locations:
Can 0: (0, 0, 1)
Can 1: (10, -0.0106598, 1)
Can 2: (9.97404, 10, 1)
Can 3: (0.0134647, 10.0156, 1)
Can 4: (-0.0769317, 4.97414, 1)
Can 5: (5.01701, 10.0561, 1)
Can 6: (-0.0425079, 19.9799, 1)

### Sameer Agarwal

Jun 23, 2023, 11:52:00 AMJun 23
Max,
Taking derivatives of squareroots is a problem at zero, other than that you are fine.
Sameer

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.

### max

Jun 24, 2023, 9:47:48 AMJun 24
to Ceres Solver
Thank you Sameer.

So if I understand we need to make sure sqrt(x) stays away from zero
because if f(x) = (x)^(1/2) then f'(x) = (1/2)*(x)^(-1/2)
and because ultimately we need to stay away from an INF result?

On my machine:
DBL_MIN = 2.22507e-308
sqrt(DBL_MIN) = 1.49167e-154
DBL_MIN^2 = 0

So if I just drop a DBL_MIN into the equation:

residual[0] = measurement_ - sqrt(DBL_MIN + (x1[0]-x2[0])*(x1[0]-x2[0]) + (y1[0]-y2[0])*(y1[0]-y2[0]) + (z1[0]-z2[0])*(z1[0]-z2[0]));

It might make him cringe, but
Sheldon will not notice the slight bias in beer can locations.
And I think it keeps me far enough away from the INF.
No?

Thank you,
Max