C++ (Cpp) States::getNumFeatures Examples

Example #1

File: states.cpp Project: hongbin8237/slam

void States::add( const States& a)
{
    if( a.getNumFeatures()!=this->getNumFeatures() )
    {
        std::cerr << "add: feature mismatch" << std::endl;
        exit(EXIT_FAILURE);
    }
    this->X += a.X;
    this->V += a.V;
    for(int i = 0; i<getNumFeatures(); i++ )
    {
        this->features[i]->set_body_position( this->features[i]->get_body_position() +
                a.features[i]->get_body_position() );
        this->features[i]->set_world_position( this->features[i]->get_world_position() +
                a.features[i]->get_world_position() );
    }
    this->b += a.b;
}