bool extractPCFromColorModel(const PCRGB::Ptr& pc_in, PCRGB::Ptr& pc_out,
Eigen::Vector3d& mean, Eigen::Matrix3d& cov, double std_devs)
{
double hue_weight;
ros::param::param<double>("~hue_weight", hue_weight, 1.0);
printf("%f\n", hue_weight);
Eigen::Vector3d x, x_m;
Eigen::Matrix3d cov_inv = cov.inverse();
for(uint32_t i=0;i<pc_in->size();i++) {
extractHSL(pc_in->points[i].rgb, x(0), x(1), x(2));
x_m = x - mean;
x_m(0) *= hue_weight;
double dist = std::sqrt(x_m.transpose() * cov_inv * x_m);
if(dist <= std_devs)
pc_out->points.push_back(pc_in->points[i]);
}
}
bool LeastSquare(const std::vector<double>& x_value, const std::vector<double>& y_value,
int M, std::vector<double>& a_value)
{
assert(x_value.size() == y_value.size());
assert(a_value.size() == M);
double *matrix = new double[M * M];
double *b= new double[M];
std::vector<double> x_m(x_value.size(), 1.0);
std::vector<double> y_i(y_value.size(), 0.0);
for(int i = 0; i < M; i++)
{
matrix[ARR_INDEX(0, i, M)] = std::accumulate(x_m.begin(), x_m.end(), 0.0);
for(int j = 0; j < static_cast<int>(y_value.size()); j++)
{
y_i[j] = x_m[j] * y_value[j];
}
b[i] = std::accumulate(y_i.begin(), y_i.end(), 0.0);
for(int k = 0; k < static_cast<int>(x_m.size()); k++)
{
x_m[k] *= x_value[k];
}
}
for(int row = 1; row < M; row++)
{
for(int i = 0; i < M - 1; i++)
{
matrix[ARR_INDEX(row, i, M)] = matrix[ARR_INDEX(row - 1, i + 1, M)];
}
matrix[ARR_INDEX(row, M - 1, M)] = std::accumulate(x_m.begin(), x_m.end(), 0.0);
for(int k = 0; k < static_cast<int>(x_m.size()); k++)
{
x_m[k] *= x_value[k];
}
}
GuassEquation equation(M, matrix, b);
delete[] matrix;
delete[] b;
return equation.Resolve(a_value);
}
/**
* Initializes the sampel sets so that the first update works appropriately
*/
MCL::MCL()
{
// Initialize particles to be randomly spread about the field...
srand(time(NULL));
for (int m = 0; m < M; ++m) {
//Particle p_m;
// X bounded by width of the field
// Y bounded by height of the field
// H between +-pi
PoseEst x_m(float(rand() % int(FIELD_WIDTH)),
float(rand() % int(FIELD_HEIGHT)),
float(((rand() % FULL_CIRC) - HALF_CIRC))*DEG_TO_RAD);
Particle p_m(x_m, 1.0f);
// p_m.pose = x_m;
// p_m.weight = 1;
X_t.push_back(p_m);
}
updateEstimates();
}
void OdeGear(
Fun &F ,
size_t m ,
size_t n ,
const Vector &T ,
Vector &X ,
Vector &e )
{
// temporary indices
size_t i, j, k;
typedef typename Vector::value_type Scalar;
// check numeric type specifications
CheckNumericType<Scalar>();
// check simple vector class specifications
CheckSimpleVector<Scalar, Vector>();
CPPAD_ASSERT_KNOWN(
m >= 1,
"OdeGear: m is less than one"
);
CPPAD_ASSERT_KNOWN(
n > 0,
"OdeGear: n is equal to zero"
);
CPPAD_ASSERT_KNOWN(
size_t(T.size()) >= (m+1),
"OdeGear: size of T is not greater than or equal (m+1)"
);
CPPAD_ASSERT_KNOWN(
size_t(X.size()) >= (m+1) * n,
"OdeGear: size of X is not greater than or equal (m+1) * n"
);
for(j = 0; j < m; j++) CPPAD_ASSERT_KNOWN(
T[j] < T[j+1],
"OdeGear: the array T is not monotone increasing"
);
// some constants
Scalar zero(0);
Scalar one(1);
// vectors required by method
Vector alpha(m + 1);
Vector beta(m + 1);
Vector f(n);
Vector f_x(n * n);
Vector x_m0(n);
Vector x_m(n);
Vector b(n);
Vector A(n * n);
// compute alpha[m]
alpha[m] = zero;
for(k = 0; k < m; k++)
alpha[m] += one / (T[m] - T[k]);
// compute beta[m-1]
beta[m-1] = one / (T[m-1] - T[m]);
for(k = 0; k < m-1; k++)
beta[m-1] += one / (T[m-1] - T[k]);
// compute other components of alpha
for(j = 0; j < m; j++)
{ // compute alpha[j]
alpha[j] = one / (T[j] - T[m]);
for(k = 0; k < m; k++)
{ if( k != j )
{ alpha[j] *= (T[m] - T[k]);
alpha[j] /= (T[j] - T[k]);
}
}
}
// compute other components of beta
for(j = 0; j <= m; j++)
{ if( j != m-1 )
{ // compute beta[j]
beta[j] = one / (T[j] - T[m-1]);
for(k = 0; k <= m; k++)
{ if( k != j && k != m-1 )
{ beta[j] *= (T[m-1] - T[k]);
beta[j] /= (T[j] - T[k]);
}
}
}
}
// evaluate f(T[m-1], x_{m-1} )
for(i = 0; i < n; i++)
x_m[i] = X[(m-1) * n + i];
F.Ode(T[m-1], x_m, f);
// solve for x_m^0
for(i = 0; i < n; i++)
{ x_m[i] = f[i];
for(j = 0; j < m; j++)
x_m[i] -= beta[j] * X[j * n + i];
x_m[i] /= beta[m];
}
x_m0 = x_m;
// evaluate partial w.r.t x of f(T[m], x_m^0)
F.Ode_dep(T[m], x_m, f_x);
// compute the matrix A = ( alpha[m] * I - f_x )
for(i = 0; i < n; i++)
{ for(j = 0; j < n; j++)
A[i * n + j] = - f_x[i * n + j];
A[i * n + i] += alpha[m];
}
// LU factor (and overwrite) the matrix A
int sign;
CppAD::vector<size_t> ip(n) , jp(n);
sign = LuFactor(ip, jp, A);
CPPAD_ASSERT_KNOWN(
sign != 0,
"OdeGear: step size is to large"
);
// Iterations of Newton's method
for(k = 0; k < 3; k++)
{
// only evaluate f( T[m] , x_m ) keep f_x during iteration
F.Ode(T[m], x_m, f);
// b = f + f_x x_m - alpha[0] x_0 - ... - alpha[m-1] x_{m-1}
for(i = 0; i < n; i++)
{ b[i] = f[i];
for(j = 0; j < n; j++)
b[i] -= f_x[i * n + j] * x_m[j];
for(j = 0; j < m; j++)
b[i] -= alpha[j] * X[ j * n + i ];
}
LuInvert(ip, jp, A, b);
x_m = b;
}
// return estimate for x( t[k] ) and the estimated error bound
for(i = 0; i < n; i++)
{ X[m * n + i] = x_m[i];
e[i] = x_m[i] - x_m0[i];
if( e[i] < zero )
e[i] = - e[i];
}
}
