void AxesLib::init(){
//Here we supposes that 10000 steps are much more than one revolution..
//(anyway, this number can be arbitrarily bigger)
int MAX_STEPS = 10000;
//We are on (0,0)
_x = 0;
_X = 0;
_x_rev = false;
_y = 0;
_rx = 0.0;
_ry = 0.0;
_enableMotors();
//Go to the 360º limit
_step(_stPin_x, true, MAX_STEPS, _s360Pin_x);
//Once there, count steps until the beginnin..
_pv_x = _step(_stPin_x, false, MAX_STEPS, _s0Pin_x);
_pgrad_x = (float) _pv_x/360;
//The same procedure but for Y axis..
_step(_stPin_y, true, MAX_STEPS, _stopPin_y);
_pv_y = _step(_stPin_y, false, MAX_STEPS, _sbottomPin_y);
_pgrad_y = (float) _pv_y/90;
_disableMotors();
//Maximum values
_X = (360*_pgrad_x);
_Y = (180*_pgrad_y);
//Auxiliary values in case of Y > 90º
_revx = 180*_pgrad_x;
_topy = 90*_pgrad_y;
}
void AxesLib::_moveXY(int x, int y, bool nodelay){
if(x>_x)
_x += _step(_stPin_x, (x>_x), (x-_x), _s360Pin_x, nodelay);
else
_x -= _step(_stPin_x, (x>_x), (_x-x), _s0Pin_x, nodelay);
if(y>_y)
_y += _step(_stPin_y, (y>_y), (y-_y), _stopPin_y, nodelay);
else
_y -= _step(_stPin_y, (y>_y), (_y-y), _sbottomPin_y, nodelay);
}
void HitStatus::step(Object *character, Dictionary env) {
if (!started) {
started = true;
_start(character, env);
if (get_script_instance()) {
Variant v1 = Variant(character);
Variant var_env = Variant(env);
const Variant* ptr[2]={&v1,&var_env};
get_script_instance()->call_multilevel(StringName("_start"),ptr,2);
}
}
_step(character, env);
if (get_script_instance()) {
Variant v1 = Variant(character);
Variant var_env = Variant(env);
const Variant* ptr[2]={&v1,&var_env};
get_script_instance()->call_multilevel(StringName("_step"),ptr,2);
}
}
void physics_update_all()
{
PhysicsInfo *info;
Entity ent;
entitypool_remove_destroyed(pool, physics_remove);
entitymap_clear(debug_draw_map);
/* simulate */
if (!timing_get_paused())
{
_update_kinematics();
_step();
}
/* synchronize transform <-> physics */
entitypool_foreach(info, pool)
{
ent = info->pool_elem.ent;
/* if transform is dirtier, move to it, else overwrite it */
if (transform_get_dirty_count(ent) != info->last_dirty_count)
{
cpBodySetVel(info->body, cpvzero);
cpBodySetAngVel(info->body, 0.0f);
cpBodySetPos(info->body, cpv_of_vec2(transform_get_position(ent)));
cpBodySetAngle(info->body, transform_get_rotation(ent));
cpSpaceReindexShapesForBody(space, info->body);
}
else if (info->type == PB_DYNAMIC)
{
transform_set_position(ent, vec2_of_cpv(cpBodyGetPos(info->body)));
transform_set_rotation(ent, cpBodyGetAngle(info->body));
}
info->last_dirty_count = transform_get_dirty_count(ent);
}
std::vector<std::vector<vision::FeaturePtr> > EKF::run(std::vector<vision::FeaturePtr> &initial_structure,
std::vector<std::vector<vision::FeaturePtr> > &features)
{
std::vector<std::vector<vision::FeaturePtr> > result;
// 1 - Estimate scale and set the initial structure
_initializeState(initial_structure);
// 2 - Predict the observation given the initial structure
double* initial_observation = new double[2 * _num_feats_init_structure];
_predictObservation(_updated_state, initial_observation); // Estimate the 2D points as projection of the 3D points
// Number of 2D Features (usually it is the same as the number of 3D points)
int number_feats_2d = features[0].size(); // == initial_structure.size() = numFeatures
// Storage for indexes of the match between 3D points in the initial structure and 2D Features observed in first frame
int *min_index = new int[number_feats_2d];
// Storage of flags that express if a feature was already assigned as a projection of a 3D point
bool *feature_assigned = new bool[number_feats_2d];
// Storage of the minimum distance between the prediction of the observation (projected points) and the observation
double *min_dist = new double[number_feats_2d];
// Number of frames
size_t steps = features.size();
// Scale feature positions to +-1
std::vector<std::vector<vision::FeaturePtr> > scaled_features;// = features;
for (size_t frame = 0; frame < steps; frame++)
{
std::vector<vision::FeaturePtr> one_frame_scaled;
for (size_t feat = 0; feat < number_feats_2d; feat++)
{
// TODO: Be careful! In all other points (Initializer::getStaticStructure and BundlerInitializer::run) we multiply by default_depth/FocalLength
// but here we divide by x_resolution
// (0,0) is at the center of the image
one_frame_scaled.push_back(features[frame][feat]->cloneAndUpdate(-(float)((features[frame][feat]->getX() - cam.getXresolution() / 2.0)
/ (double)cam.getXresolution()), (float)((features[frame][feat]->getY() - cam.getYresolution() / 2.0)
/ (double)cam.getXresolution())));
feature_assigned[feat] = false;
}
scaled_features.push_back(one_frame_scaled);
}
// 3 - Find matching features between the projected initial structure and the Features of the first frame
double x_observed = 0, y_observed = 0, x_predicted = 0, y_predicted = 0;
for (size_t k = 0; k < number_feats_2d; k++) // All 2D Features in first frame
{
min_index[k] = -1; // Mark as not assigned
min_dist[k] = 5.0 / (double)cam.getXresolution(); // Set to a big error as initial min distance
for (int k2 = 0; k2 < _num_feats_init_structure; k2++) // All projected Features of the initial structure
{
// Estimate the distance between observation an the projection of the initial structure
x_observed = scaled_features[0][k]->getX();
x_predicted = initial_observation[k2 * 2 + 0];
y_observed = scaled_features[0][k]->getY();
y_predicted = initial_observation[k2 * 2 + 1];
double dist = (x_observed - x_predicted) * (x_observed - x_predicted) + (y_observed - y_predicted) * (y_observed
- y_predicted);
/* Check if the distance is smaller than 1 pixel and if this projected point was not already assigned.
* If it was already assigned, check if this assignement had smaller/bigger distance than the distance between
* the points in this iteration.
*/
if ((dist < min_dist[k]) && (dist < 1.0 / (double)cam.getXresolution()))
{
if (min_index[k] != -1)
{
feature_assigned[min_index[k]] = false;
}
min_index[k] = k2; // Store the matching
min_dist[k] = dist; // Store the new minimum distance
feature_assigned[k2] = true; // Store that this 3D point is matched
}
}
}
// remove the observations that have no match to the initial structure
std::vector<std::vector<vision::FeaturePtr> > reordered_features;
// for all frames
for (size_t frame = 0; frame < steps; frame++)
{
std::vector<vision::FeaturePtr> frame_features;
// for all features
for (size_t k = 0; k < features[0].size(); k++)
{
if (min_index[k] != -1)
{
frame_features.push_back(scaled_features[frame][k]);
}
}
reordered_features.push_back(frame_features);
}
// remove the 3D points from the initial structure that have no match to the observations
std::vector<vision::FeaturePtr> reordered_init_structure;
// for all points
for (size_t k = 0; k < features[0].size(); k++)
{
if (min_index[k] != -1)
{
reordered_init_structure.push_back(initial_structure[min_index[k]]);
}
}
// new number of features
_num_feats_init_structure = reordered_init_structure.size();
ROS_INFO("[EKF::run] Found %d matches for EKF.", _num_feats_init_structure);
if (_num_feats_init_structure == 0)
{
// ROS_ERROR("[EKF::run] EKF could NOT find matching features.");
// for(int nf = 0; nf < features.size(); nf++)
// {
// std::vector<vision::FeaturePtr> one_frame;
// for(int ff = 0; ff<initial_structure.size(); ff++)
// {
// vision::FeaturePtr one_feat = initial_structure.at(ff)->clone();
// one_frame.push_back(one_feat);
// }
// result.push_back(one_frame);
// }
return result;
}
// delete the old state vector
delete _updated_state;
// initalize the state vector with the initial structure that matches the observed features
_initializeState(reordered_init_structure);
// do the ekf ...
_initVariables();
int runs;
double error = 0;
// Stepping the EKF
for (size_t frame = 0; (frame < steps); frame++)
{
runs = 0;
// run the ekf with the same observation several times to reduce the prediction error
do
{
clock_t start = clock();
error = _step(reordered_features[frame]);
runs++;
} while ((error > _min_innovation_ekf) && (runs < _max_num_loops_ekf)); // terminate after max_runs or when the mean innovation is less than max_error
result.push_back(_get3DPoints());
//TODO: New! Is it working fine?
for (int idx = 0; idx < this->_num_feats_init_structure; idx++)
{
result.rbegin()->at(idx)->setId(reordered_init_structure.at(idx)->getId());
}
}
_freeVariables();
return result;
}
auto step(Re reduction, In&& input)
{
return _step(reduction, std::forward<In>(input));
}
auto _step(Re&& reduction, In&& input)
{
return _step(std::forward<Re>(reduction), std::forward<In>(input), helpers::gen_seq<sizeof...(Rdrs)>{});
}
size_t _parseGDBMessage(struct GDBStub* stub, const char* message) {
uint8_t checksum = 0;
int parsed = 1;
switch (*message) {
case '+':
stub->lineAck = GDB_ACK_RECEIVED;
return parsed;
case '-':
stub->lineAck = GDB_NAK_RECEIVED;
return parsed;
case '$':
++message;
break;
case '\x03':
mDebuggerEnter(&stub->d, DEBUGGER_ENTER_MANUAL, 0);
return parsed;
default:
_nak(stub);
return parsed;
}
int i;
char messageType = message[0];
for (i = 0; message[i] != '#'; ++i, ++parsed) {
checksum += message[i];
}
if (!message[i]) {
_nak(stub);
return parsed;
}
++i;
++parsed;
if (!message[i]) {
_nak(stub);
return parsed;
} else if (!message[i + 1]) {
++parsed;
_nak(stub);
return parsed;
}
parsed += 2;
int networkChecksum = _hex2int(&message[i], 2);
if (networkChecksum != checksum) {
mLOG(DEBUGGER, WARN, "Checksum error: expected %02x, got %02x", checksum, networkChecksum);
_nak(stub);
return parsed;
}
_ack(stub);
++message;
switch (messageType) {
case '?':
snprintf(stub->outgoing, GDB_STUB_MAX_LINE - 4, "S%02x", SIGINT);
_sendMessage(stub);
break;
case 'c':
_continue(stub, message);
break;
case 'G':
_writeGPRs(stub, message);
break;
case 'g':
_readGPRs(stub, message);
break;
case 'H':
// This is faked because we only have one thread
strncpy(stub->outgoing, "OK", GDB_STUB_MAX_LINE - 4);
_sendMessage(stub);
break;
case 'M':
_writeMemory(stub, message);
break;
case 'm':
_readMemory(stub, message);
break;
case 'P':
_writeRegister(stub, message);
break;
case 'p':
_readRegister(stub, message);
break;
case 'Q':
_processQWriteCommand(stub, message);
break;
case 'q':
_processQReadCommand(stub, message);
break;
case 's':
_step(stub, message);
break;
case 'V':
_processVWriteCommand(stub, message);
break;
case 'v':
_processVReadCommand(stub, message);
break;
case 'X':
_writeMemoryBinary(stub, message);
break;
case 'Z':
_setBreakpoint(stub, message);
break;
case 'z':
_clearBreakpoint(stub, message);
break;
default:
_error(stub, GDB_UNSUPPORTED_COMMAND);
break;
}
return parsed;
}
BehaviorNode::Status BehaviorNode::step(const Variant& target, Dictionary env) {
return _step(target, env);
}
