Seeking help with my can locator problem
34 views
Skip to first unread message
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?
// 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"
// on the best answer...
// 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)
{
google::InitGoogleLogging(argv[0]);
// 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.AddParameterBlock(&locations[0].x, 1);
problem.AddParameterBlock(&locations[0].y, 1);
problem.AddParameterBlock(&locations[0].z, 1);
problem.SetParameterBlockConstant(&locations[0].x);
problem.SetParameterBlockConstant(&locations[0].y);
problem.SetParameterBlockConstant(&locations[0].z);
problem.AddParameterBlock(&locations[1].x, 1);
problem.AddParameterBlock(&locations[1].y, 1);
problem.AddParameterBlock(&locations[1].z, 1);
problem.SetParameterBlockConstant(&locations[1].x);
// problem.SetParameterBlockConstant(&locations[1].y);
problem.SetParameterBlockConstant(&locations[1].z);
problem.AddParameterBlock(&locations[2].x, 1);
problem.AddParameterBlock(&locations[2].y, 1);
problem.AddParameterBlock(&locations[2].z, 1);
// problem.SetParameterBlockConstant(&locations[2].x);
problem.SetParameterBlockConstant(&locations[2].y);
problem.SetParameterBlockConstant(&locations[2].z);
// Add parameter blocks for the unknown cans
problem.AddParameterBlock(&locations[3].x, 1);
problem.AddParameterBlock(&locations[3].y, 1);
problem.AddParameterBlock(&locations[3].z, 1);
// problem.SetParameterBlockConstant(&locations[3].z);
problem.AddParameterBlock(&locations[4].x, 1);
problem.AddParameterBlock(&locations[4].y, 1);
problem.AddParameterBlock(&locations[4].z, 1);
// problem.SetParameterBlockConstant(&locations[4].z);
problem.AddParameterBlock(&locations[5].x, 1);
problem.AddParameterBlock(&locations[5].y, 1);
problem.AddParameterBlock(&locations[5].z, 1);
// problem.SetParameterBlockConstant(&locations[5].z);
problem.AddParameterBlock(&locations[6].x, 1);
problem.AddParameterBlock(&locations[6].y, 1);
problem.AddParameterBlock(&locations[6].z, 1);
// 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;
problem.AddResidualBlock(
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;
}
// 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"
// on the best answer...
// 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)
{
google::InitGoogleLogging(argv[0]);
// 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.AddParameterBlock(&locations[0].x, 1);
problem.AddParameterBlock(&locations[0].y, 1);
problem.AddParameterBlock(&locations[0].z, 1);
problem.SetParameterBlockConstant(&locations[0].x);
problem.SetParameterBlockConstant(&locations[0].y);
problem.SetParameterBlockConstant(&locations[0].z);
problem.AddParameterBlock(&locations[1].x, 1);
problem.AddParameterBlock(&locations[1].y, 1);
problem.AddParameterBlock(&locations[1].z, 1);
problem.SetParameterBlockConstant(&locations[1].x);
// problem.SetParameterBlockConstant(&locations[1].y);
problem.SetParameterBlockConstant(&locations[1].z);
problem.AddParameterBlock(&locations[2].x, 1);
problem.AddParameterBlock(&locations[2].y, 1);
problem.AddParameterBlock(&locations[2].z, 1);
// problem.SetParameterBlockConstant(&locations[2].x);
problem.SetParameterBlockConstant(&locations[2].y);
problem.SetParameterBlockConstant(&locations[2].z);
// Add parameter blocks for the unknown cans
problem.AddParameterBlock(&locations[3].x, 1);
problem.AddParameterBlock(&locations[3].y, 1);
problem.AddParameterBlock(&locations[3].z, 1);
// problem.SetParameterBlockConstant(&locations[3].z);
problem.AddParameterBlock(&locations[4].x, 1);
problem.AddParameterBlock(&locations[4].y, 1);
problem.AddParameterBlock(&locations[4].z, 1);
// problem.SetParameterBlockConstant(&locations[4].z);
problem.AddParameterBlock(&locations[5].x, 1);
problem.AddParameterBlock(&locations[5].y, 1);
problem.AddParameterBlock(&locations[5].z, 1);
// problem.SetParameterBlockConstant(&locations[5].z);
problem.AddParameterBlock(&locations[6].x, 1);
problem.AddParameterBlock(&locations[6].y, 1);
problem.AddParameterBlock(&locations[6].z, 1);
// 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;
problem.AddResidualBlock(
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:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
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
Threads 1 1
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)
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
Threads 1 1
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
to ceres-...@googlegroups.com
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.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/ba875a8c-0fd8-4d5c-a02b-dbf8251606efn%40googlegroups.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
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
Sameer Agarwal
Jun 28, 2023, 8:07:52 PMJun 28
to ceres-...@googlegroups.com
Max,
You can certainly do what you are suggesting. Given that your data is only accurate to a couple of decimals, having a perturbation of 1e-154 is going to be okay :)
Sameer
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/b672d862-df68-414b-8547-72b2d57046d6n%40googlegroups.com.
Reply all
Reply to author
Forward
0 new messages