Example #1
0
/* This is called when a new ICE connection is made.  It arranges for
   the ICE connection to be handled via the event loop.  */
static void
iceNewConnection(IceConn connection,
                 IcePointer clientData,
                 Bool opening,
                 IcePointer *watchData)
{
   if (opening)
     {
        SM_DEBUG(printf("ICE connection opening\n"));

        /* Make sure we don't pass on these file descriptors to any
           exec'ed children */
        fcntl(IceConnectionNumber(connection), F_SETFD,
              fcntl(IceConnectionNumber(connection),
                    F_GETFD, 0) | FD_CLOEXEC);

        iceWatchFdHandle = compAddWatchFd(IceConnectionNumber(connection),
                                          POLLIN | POLLPRI | POLLHUP | POLLERR,
                                          iceProcessMessages, connection);

        iceConnected = 1;
     }
   else
     {
        SM_DEBUG(printf("ICE connection closing\n"));

        if (iceConnected)
          {
             compRemoveWatchFd(iceWatchFdHandle);

             iceWatchFdHandle = 0;
             iceConnected = 0;
          }
     }
}
    void RansacProblem<M>::getSamples(int &iterations, std::vector<int> &samples)
    {
        // We're assuming that indices_ have already been set in the constructor
        if (indices_->size() < (size_t)getSampleSize())
        {
          SM_ERROR ("[sm::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
                     samples.size (), indices_->size ());
          // one of these will make it stop :)
          samples.clear();
          iterations = INT_MAX - 1;
          return;
        }

        // Get a second point which is different than the first
        samples.resize( getSampleSize() );
        for (int iter = 0; iter < max_sample_checks_; ++iter)
        {
            drawIndexSample (samples);
          
          // If it's a good sample, stop here
          if (isSampleGood (samples))
            return;
        }
        SM_DEBUG ("[sm::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
        samples.clear ();

    }
Example #3
0
/* This is called when data is available on an ICE connection. */
static bool
iceProcessMessages (IceConn connection)
{
    SM_DEBUG (printf ("ICE connection process messages\n"));

    IceProcessMessagesStatus status = IceProcessMessages (connection, NULL, NULL);

    if (status == IceProcessMessagesIOError)
    {
	SM_DEBUG (printf ("ICE connection process messages"
			  " - error => shutting down the connection\n"));

	IceSetShutdownNegotiation (connection, False);
	IceCloseConnection (connection);
    }

    return 1;
}
Example #4
0
    bool Msac<PROBLEM_T>::computeModel(int debug_verbosity_level)
    {
        typedef PROBLEM_T problem_t;
        typedef typename problem_t::model_t model_t;

        // Warn and exit if no threshold was set
        if (threshold_ == std::numeric_limits<double>::max())
        {
            SM_ERROR ("[sm::MEstimatorSampleConsensus::computeModel] No threshold set!\n");
            return (false);
        }

        iterations_ = 0;
        double d_best_penalty = std::numeric_limits<double>::max();
        double k = 1.0;

        std::vector<int> best_model;
        std::vector<int> selection;
        model_t model_coefficients;
        std::vector<double> distances;

        int n_inliers_count = 0;
        unsigned skipped_count = 0;
        // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
        const unsigned max_skip = max_iterations_ * 10;
  
        // Iterate
        while (iterations_ < k && skipped_count < max_skip)
        {
            // Get X samples which satisfy the model criteria
            sac_model_->getSamples (iterations_, selection);

            if (selection.empty ()) break;

            // Search for inliers in the point cloud for the current plane model M
            if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
            {
                //iterations_++;
                ++ skipped_count;
                continue;
            }

            double d_cur_penalty = 0;
            // Iterate through the 3d points and calculate the distances from them to the model
            sac_model_->getDistancesToModel (model_coefficients, distances);
    
            if (distances.empty () && k > 1.0)
                continue;

            for (size_t i = 0; i < distances.size (); ++i)
                d_cur_penalty += (std::min) (distances[i], threshold_);

            // Better match ?
            if (d_cur_penalty < d_best_penalty)
            {
                d_best_penalty = d_cur_penalty;

                // Save the current model/coefficients selection as being the best so far
                model_              = selection;
                model_coefficients_ = model_coefficients;

                n_inliers_count = 0;
                // Need to compute the number of inliers for this model to adapt k
                for (size_t i = 0; i < distances.size (); ++i)
                    if (distances[i] <= threshold_)
                        ++n_inliers_count;

                // Compute the k parameter (k=log(z)/log(1-w^n))
                double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
                double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
                p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
                p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
                k = log (1.0 - probability_) / log (p_no_outliers);
            }

            ++iterations_;
            if (debug_verbosity_level > 1)
                SM_DEBUG ("[sm::MEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast<int> (ceil (k)), d_best_penalty);
            if (iterations_ > max_iterations_)
            {
                if (debug_verbosity_level > 0)
                    SM_DEBUG ("[sm::MEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials.\n");
                break;
            }
        }

        if (model_.empty ())
        {
            if (debug_verbosity_level > 0)
                SM_DEBUG ("[sm::MEstimatorSampleConsensus::computeModel] Unable to find a solution!\n");
            return (false);
        }

        // Iterate through the 3d points and calculate the distances from them to the model again
        sac_model_->getDistancesToModel (model_coefficients_, distances);
        std::vector<int> &indices = *sac_model_->getIndices ();

        if (distances.size () != indices.size ())
        {
            SM_ERROR ("[sm::MEstimatorSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ());
            return (false);
        }

        inliers_.resize (distances.size ());
        // Get the inliers for the best model found
        n_inliers_count = 0;
        for (size_t i = 0; i < distances.size (); ++i)
            if (distances[i] <= threshold_)
                inliers_[n_inliers_count++] = indices[i];

        // Resize the inliers vector
        inliers_.resize (n_inliers_count);

        if (debug_verbosity_level > 0)
            SM_DEBUG ("[sm::MEstimatorSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count);

        return (true);

    }