void fullDemo() {
int robotState = 0;
uint16_t sensorPattern[5] = {0};
while(robotState != 6) {
switch(robotState) {
case 0:
while(get_coord_x() < 200) {
forwards(20);
}
robotState = 1;
break;
case 1:
setSensorSide(1);
while(get_coord_x() < 400) {
correctForwardMotion();
}
robotState = 2;
break;
case 2:
while(convertToDeg(get_theta()) > -90) {
spinLeft();
}
while(get_coord_y() < 120) {
forwards(20);
}
while(convertToDeg(get_theta()) < 0) {
spinRight();
}
while(get_coord_x() < 600) {
forwards(20);
}
robotState = 3;
break;
case 3:
setSensorSide(2);
while(get_coord_x() < 800) {
correctForwardMotion();
}
robotState = 4;
break;
case 4:
forwards(20);
while(sensorPattern[3]<2000){
getRawSensors(sensorPattern);
}
robotState = 5;
break;
case 5:
followLine();
robotState = 6;
break;
}
}
}
void RegistrationResult::show(std::ostream &out) const {
algebra::VectorD<4> quaternion = R_.get_quaternion();
out << "Name: " << get_name() << " Image index: " << get_image_index()
<< " Projection index: " << get_projection_index()
<< " (Phi,Theta,Psi) = ( " << get_phi() << " , " << get_theta() << " , "
<< get_psi() << " ) | Shift (x,y) " << get_shift()
<< " CCC = " << get_ccc() << " Quaternion " << quaternion;
}
//! Writes a result line to a file
void RegistrationResult::write(std::ostream &out) const {
algebra::VectorD<4> quaternion = R_.get_quaternion();
char c = '|';
out << get_image_index() << c << get_projection_index() << c << get_phi() << c
<< get_theta() << c << get_psi() << c << quaternion[0] << c
<< quaternion[1] << c << quaternion[2] << c << quaternion[3] << c
<< get_shift()[0] << c << get_shift()[1] << c << get_ccc() << c
<< std::endl;
}
/*!
Print to stdout the values of the current visual feature.
\param select : Selection of a subset of the possible 2D image point
feature coordinates.
- To print all the two polar coordinates \f$(\rho,\theta)\f$ used as
features use vpBasicFeature::FEATURE_ALL.
- To print only one of the polar coordinate
feature \f$(\rho,\theta)\f$ use one of the
corresponding function selectRho() or selectTheta().
\code
// Creation of the current feature s
vpFeaturePointPolar s;
s.buildFrom(0.1, M_PI_2, 1.3);
s.print(); // print all the 2 components of the image point feature
s.print(vpBasicFeature::FEATURE_ALL); // same behavior then previous line
s.print(vpFeaturePointPolar::selectRho()); // print only the rho component
\endcode
*/
void
vpFeaturePointPolar::print(const unsigned int select ) const
{
std::cout <<"Point: Z=" << get_Z() ;
if (vpFeaturePointPolar::selectRho() & select )
std::cout << " rho=" << get_rho() ;
if (vpFeaturePointPolar::selectTheta() & select )
std::cout << " theta=" << get_theta() ;
std::cout <<std::endl ;
}
/*!
Display image point feature.
\param cam : Camera parameters.
\param I : Image.
\param color : Color to use for the display
\param thickness : Thickness of the feature representation.
*/
void
vpFeaturePointPolar::display(const vpCameraParameters &cam,
const vpImage<unsigned char> &I,
const vpColor &color,
unsigned int thickness) const
{
try {
double rho,theta;
rho = get_rho();
theta = get_theta();
double x,y;
x = rho*cos(theta);
y = rho*sin(theta);
vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
}
catch(...) {
vpERROR_TRACE("Error caught") ;
throw ;
}
}
/*!
Compute and return the interaction matrix \f$ L \f$ associated to a
subset of the possible 2D image point features with polar
coordinates \f$(\rho,\theta)\f$.
\f[
L = \left[
\begin{array}{l}
L_{\rho} \\
\; \\
L_{\theta}\\
\end{array}
\right]
=
\left[
\begin{array}{cccccc}
\frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} & (1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\
\; \\
\frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta /\rho & \sin\theta/\rho & -1 \\
\end{array}
\right]
\f]
where \f$Z\f$ is the 3D depth of the considered point.
\param select : Selection of a subset of the possible polar
point coordinate features.
- To compute the interaction matrix for all the two
subset features \f$(\rho,\theta)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [2 \times
6] \f$
- To compute the interaction matrix for only one of the subset
(\f$\rho,\theta\f$) use one of the corresponding function
selectRho() or selectTheta(). In that case the returned
interaction matrix is \f$ [1 \times 6] \f$ dimension.
\return The interaction matrix computed from the 2D point
polar coordinate features.
\exception vpFeatureException::badInitializationError : If the point
is behind the camera \f$(Z < 0)\f$, or if the 3D depth is null \f$(Z
= 0)\f$, or if the \f$\rho\f$ polar coordinate of the point is null.
The code below shows how to compute the interaction matrix associated to
the visual feature \f$s = (\rho,\theta)\f$.
\code
vpFeaturePointPolar s;
double rho = 0.3;
double theta = M_PI;
double Z = 1;
// Creation of the current feature s
s.buildFrom(rho, theta, Z);
// Build the interaction matrix L_s
vpMatrix L = s.interaction();
\endcode
The interaction matrix could also be build by:
\code
vpMatrix L = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
In both cases, L is a 2 by 6 matrix. The first line corresponds to
the \f$\rho\f$ visual feature while the second one to the
\f$\theta\f$ visual feature.
It is also possible to build the interaction matrix associated to
one of the possible features. The code below shows how to consider
only the \f$\theta\f$ component.
\code
vpMatrix L_theta = s.interaction( vpFeaturePointPolar::selectTheta() );
\endcode
In that case, L_theta is a 1 by 6 matrix.
*/
vpMatrix
vpFeaturePointPolar::interaction(const unsigned int select)
{
vpMatrix L ;
L.resize(0,6) ;
if (deallocate == vpBasicFeature::user)
{
for (unsigned int i = 0; i < nbParameters; i++)
{
if (flags[i] == false)
{
switch(i){
case 0:
vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
break;
case 1:
vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
break;
case 2:
vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
}
}
resetFlags();
}
double rho = get_rho() ;
double theta = get_theta() ;
double Z_ = get_Z() ;
double c_ = cos(theta);
double s_ = sin(theta);
double rho2 = rho*rho;
if (fabs(rho) < 1e-6) {
vpERROR_TRACE("rho polar coordinate of the point is null") ;
std::cout <<"rho = " << rho << std::endl ;
throw(vpFeatureException(vpFeatureException::badInitializationError,
"rho polar coordinate of the point is null")) ;
}
if (Z_ < 0)
{
vpERROR_TRACE("Point is behind the camera ") ;
std::cout <<"Z = " << Z_ << std::endl ;
throw(vpFeatureException(vpFeatureException::badInitializationError,
"Point is behind the camera ")) ;
}
if (fabs(Z_) < 1e-6)
{
vpERROR_TRACE("Point Z coordinates is null ") ;
std::cout <<"Z = " << Z_ << std::endl ;
throw(vpFeatureException(vpFeatureException::badInitializationError,
"Point Z coordinates is null")) ;
}
if (vpFeaturePointPolar::selectRho() & select )
{
vpMatrix Lrho(1,6) ; Lrho = 0;
Lrho[0][0] = -c_/Z_ ;
Lrho[0][1] = -s_/Z_ ;
Lrho[0][2] = rho/Z_ ;
Lrho[0][3] = (1+rho2)*s_ ;
Lrho[0][4] = -(1+rho2)*c_ ;
Lrho[0][5] = 0 ;
// printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
// std::cout << "Lrho: " << Lrho << std::endl;
L = vpMatrix::stackMatrices(L,Lrho) ;
}
if (vpFeaturePointPolar::selectTheta() & select )
{
vpMatrix Ltheta(1,6) ; Ltheta = 0;
Ltheta[0][0] = s_/(rho*Z_) ;
Ltheta[0][1] = -c_/(rho*Z_) ;
Ltheta[0][2] = 0 ;
Ltheta[0][3] = c_/rho ;
Ltheta[0][4] = s_/rho ;
Ltheta[0][5] = -1 ;
// printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
// std::cout << "Ltheta: " << Ltheta << std::endl;
L = vpMatrix::stackMatrices(L,Ltheta) ;
}
return L ;
}
double vert(coordinate d, double f, double R, double t, double r, double g){
double s;
double tmp;
double rt = R-t; //안쪽 반지름
char st; //첫 gap(삼각형일때)
double tx1, tx2, ty1, ty2; //임시변수
double th; //각도
double savedx = d.x;
s = 0;
st = 1;
while(d.x*d.x + d.y*d.y < rt*rt){
if((d.y+g)*(d.y+g)+(d.x+g)*(d.x+g) > rt*rt){
if((d.x+g)*(d.x+g)+d.y*d.y > rt*rt){
if(st){
tx1 = get_x(d.y, rt);
ty1 = rt / sqrt(2);
tx2 = get_x(ty1, rt);
th = get_theta(tx1, d.y, tx2, ty1, rt);
tmp = ((ty1 - d.y)*(tx1 - d.x) / 2.0); // 삼각형 면적
tmp += area_arc(th, rt);
st = 0;
}else{
if((d.y+g)*(d.y+g)+d.x*d.x > rt*rt){
tx1 = get_x(d.y, rt);
ty1 = get_y(d.x, rt);
th = get_theta(tx1, d.y, d.x, ty1, rt);
tmp = (tx1-d.x)*(ty1-d.y)/2 + area_arc(th, rt);
}else{
double a1, a2;
tx1 = get_x(d.y, rt);
a1 = (tx1-d.x)*g/2.0;
tx2 = get_x(d.y+g, rt);
a2 = (tx2-d.x)*g/2.0;
tmp = a1+a2;
th = get_theta(tx1, d.y, tx2, d.y+g, rt);
tmp += area_arc(th, rt);
}
}
}else{
if(st){
double a1, a2, a3;
tx1 = d.x+g;
ty2 = rt/sqrt(2.0);
tx2 = get_x(ty2, rt);
ty1 = get_y(tx1, rt);
a1 = (tx2 - d.x)*(ty2-d.y)/2.0;
a2 = (ty1-d.y)*(tx1-tx2);
a3 = (ty2-ty1)*(tx1-tx2)/2.0;
th = get_theta(tx1, ty1, tx2, ty2, rt);
tmp = a1+a2+a3+area_arc(th, rt);
st = 0;
}else{
double a1, a2, a3;
tx1 = d.x+g;
ty1 = get_y(tx1, rt);
ty2 = d.y+g;
tx2 = get_x(ty2, rt);
a1 = (tx2-d.x)*g;
a2 = (ty1-d.y)*(d.x+g-tx2);
a3 = (ty2-ty1)*(tx1-tx2)/2.0;
th = get_theta(tx1, ty1, tx2, ty2, rt);
tmp = a1+a2+a3+area_arc(th, rt);
}
}
} else{
if(st){
tmp = g*g/2.0;
st = 0;
}else{
tmp = g*g;
}
}
d.x += g+2*r;
s += tmp;
}
d.x = savedx;
return s;
}
pred_model_t *
train(data_t *train_data, int initialization, int method, params_t *params)
{
int chain, default_rule;
pred_model_t *pred_model;
ruleset_t *rs, *rs_temp;
double max_pos, pos_temp, null_bound;
pred_model = NULL;
rs = NULL;
if (compute_pmf(train_data->nrules, params) != 0)
goto err;
compute_cardinality(train_data->rules, train_data->nrules);
if (compute_log_gammas(train_data->nsamples, params) != 0)
goto err;
if ((pred_model = calloc(1, sizeof(pred_model_t))) == NULL)
goto err;
default_rule = 0;
if (ruleset_init(1,
train_data->nsamples, &default_rule, train_data->rules, &rs) != 0)
goto err;
max_pos = compute_log_posterior(rs, train_data->rules,
train_data->nrules, train_data->labels, params, 1, -1, &null_bound);
if (permute_rules(train_data->nrules) != 0)
goto err;
for (chain = 0; chain < params->nchain; chain++) {
rs_temp = run_mcmc(params->iters,
train_data->nsamples, train_data->nrules,
train_data->rules, train_data->labels, params, max_pos);
pos_temp = compute_log_posterior(rs_temp, train_data->rules,
train_data->nrules, train_data->labels, params, 1, -1,
&null_bound);
if (pos_temp >= max_pos) {
ruleset_destroy(rs);
rs = rs_temp;
max_pos = pos_temp;
} else {
ruleset_destroy(rs_temp);
}
}
pred_model->theta =
get_theta(rs, train_data->rules, train_data->labels, params);
pred_model->rs = rs;
rs = NULL;
/*
* THIS IS INTENTIONAL -- makes error handling localized.
* If we branch to err, then we want to free an allocated model;
* if we fall through naturally, then we don't.
*/
if (0) {
err:
if (pred_model != NULL)
free (pred_model);
}
/* Free allocated memory. */
if (log_lambda_pmf != NULL)
free(log_lambda_pmf);
if (log_eta_pmf != NULL)
free(log_eta_pmf);
if (rule_permutation != NULL)
free(rule_permutation);
if (log_gammas != NULL)
free(log_gammas);
if (rs != NULL)
ruleset_destroy(rs);
return (pred_model);
}
