AnimatorType::AnimatorType(const QString& path, const QString& id ):
CartaObject( CLASS_NAME, path, id ){
m_select = nullptr;
m_removed = false;
m_visible = true;
_initializeState();
_makeSelection();
_initializeCommands();
}
Region::Region(const QString& path, const QString& id )
:CartaObject( CLASS_NAME, path, id ){
_initializeCallbacks();
_initializeState();
}
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;
}
Layer::Layer( const QString& className, const QString& path, const QString& id) :
CartaObject( className, path, id){
m_renderQueued = false;
_initializeSingletons();
_initializeState();
}
Contour::Contour() :
m_state(""){
_initializeSingletons();
_initializeState( );
}
