implemented faster approximate observation model

wumanu · wumanu · commit c4851018e580 · 2014-09-28T00:03:44.000+02:00
diff --git a/distribution_test.m b/distribution_test.m
@@ -6,7 +6,7 @@
 density = importdata('/tmp/distribution_test/evaluation.txt');
 
 %%
-bin_count = 100;
+bin_count = 1000;
 bin_width = (max(samples) - min(samples)) / bin_count;
 approximate_density = hist(samples, bin_count) / (size(samples,1) * bin_width);
 
diff --git a/include/fast_filtering/utils/helper_functions.hpp b/include/fast_filtering/utils/helper_functions.hpp
@@ -990,52 +990,128 @@ template <typename T> class LogSumExp
 class DiscreteSampler
 {
 public:
-	template <typename T> DiscreteSampler(std::vector<T> log_likelihoods)
+    template <typename T> DiscreteSampler(std::vector<T> log_prob)
 	{
 		fibo_.seed(RANDOM_SEED);
 
-		// compute the likelihoods and normalize them ------------------------------------------------------------------------------
-        sorted_indices_ = ff::hf::SortDescend(log_likelihoods);
-		double max = log_likelihoods[sorted_indices_[0]];
-		for(int i = 0; i < int(log_likelihoods.size()); i++)
-			log_likelihoods[i] -= max;
+        // compute the prob and normalize them
+        sorted_indices_ = ff::hf::SortDescend(log_prob);
+        double max = log_prob[sorted_indices_[0]];
+        for(int i = 0; i < int(log_prob.size()); i++)
+            log_prob[i] -= max;
 
-		std::vector<double> likelihoods(log_likelihoods.size());
+        std::vector<double> prob(log_prob.size());
 		double sum = 0;
-		for(int i = 0; i < int(log_likelihoods.size()); i++)
+        for(int i = 0; i < int(log_prob.size()); i++)
 		{
-			likelihoods[i] = exp(log_likelihoods[i]);
-			sum += likelihoods[i];
+            prob[i] = exp(log_prob[i]);
+            sum += prob[i];
 		}
-		for(int i = 0; i < int(likelihoods.size()); i++)
-			likelihoods[i] /= sum;
-
-		// compute the cumulative likelihood ------------------------------------------------------------------
-		cum_likelihoods_.resize(log_likelihoods.size());
-		cum_likelihoods_[0] = likelihoods[sorted_indices_[0]];
-		for(int i = 1; i < int(log_likelihoods.size()); i++)
-			cum_likelihoods_[i] = cum_likelihoods_[i-1] + likelihoods[sorted_indices_[i]];
+        for(int i = 0; i < int(prob.size()); i++)
+            prob[i] /= sum;
+
+        // compute the cumulative probability
+        cumulative_prob_.resize(log_prob.size());
+        cumulative_prob_[0] = prob[sorted_indices_[0]];
+        for(int i = 1; i < int(log_prob.size()); i++)
+            cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]];
 	}
 
 	~DiscreteSampler() {}
 
     int Sample()
 	{
-		double rand_number = fibo_();
+        double uniform_sample = fibo_();
 		int sample_index = 0;
-		while(rand_number > cum_likelihoods_[sample_index]) sample_index++;
+        while(uniform_sample > cumulative_prob_[sample_index]) sample_index++;
 
 		return sorted_indices_[sample_index];
 	}
 
+
+
+    int MapStandardGaussian(double gaussian_sample) const
+    {
+        // map from a gaussian to a uniform distribution
+        double uniform_sample = 0.5 *
+                (1.0 + std::erf(gaussian_sample / std::sqrt(2.0)));
+        int sample_index = 0;
+        while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; //TODO: THIS COULD BE DONE IN LOG TIME
+
+        return sorted_indices_[sample_index];
+    }
+
 private:
 	boost::lagged_fibonacci607 fibo_;
 	std::vector<int> sorted_indices_;
-	std::vector<double> cum_likelihoods_;
+    std::vector<double> cumulative_prob_;
 };
 
 
 
+// the above class should go away
+class DiscreteDistribution
+{
+public:
+    template <typename T> DiscreteDistribution(std::vector<T> log_prob)
+    {
+        uniform_sampler_.seed(RANDOM_SEED);
+
+        // substract max to avoid numerical issues
+        double max = hf::bound_value(log_prob, true);
+        for(int i = 0; i < int(log_prob.size()); i++)
+            log_prob[i] -= max;
+
+        // compute probabilities
+        std::vector<double> prob(log_prob.size());
+        double sum = 0;
+        for(int i = 0; i < int(log_prob.size()); i++)
+        {
+            prob[i] = std::exp(log_prob[i]);
+            sum += prob[i];
+        }
+        for(int i = 0; i < int(prob.size()); i++)
+            prob[i] /= sum;
+
+        // compute the cumulative probability
+        cumulative_prob_.resize(prob.size());
+        cumulative_prob_[0] = prob[0];
+        for(size_t i = 1; i < prob.size(); i++)
+            cumulative_prob_[i] = cumulative_prob_[i-1] + prob[i];
+    }
+
+    ~DiscreteDistribution() {}
+
+    int Sample()
+    {
+        return MapStandardUniform(uniform_sampler_());
+    }
+
+    int MapStandardGaussian(double gaussian_sample) const
+    {
+        double uniform_sample =
+                0.5 * (1.0 + std::erf(gaussian_sample / std::sqrt(2.0)));
+        return MapStandardUniform(uniform_sample);
+    }
+
+    int MapStandardUniform(double uniform_sample) const
+    {
+        std::vector<double>::const_iterator iterator =
+                                 std::lower_bound(cumulative_prob_.begin(),
+                                                  cumulative_prob_.end(),
+                                                  uniform_sample);
+
+        return iterator - cumulative_prob_.begin();
+    }
+
+private:
+    boost::lagged_fibonacci607  uniform_sampler_;
+    std::vector<double>         cumulative_prob_;
+};
+
+
+
+
 
 
 
diff --git a/include/pose_tracking/models/observation_models/approximate_kinect_pixel_observation_model.hpp b/include/pose_tracking/models/observation_models/approximate_kinect_pixel_observation_model.hpp
@@ -0,0 +1,218 @@
+/*************************************************************************
+This software allows for filtering in high-dimensional observation and
+state spaces, as described in
+
+M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal.
+Probabilistic Object Tracking using a Range Camera
+IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013
+
+In a publication based on this software pleace cite the above reference.
+
+
+Copyright (C) 2014  Manuel Wuthrich
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*************************************************************************/
+
+#ifndef POSE_TRACKING_MODELS_OBSERVATION_MODELS_APPROXIMATE_KINECT_PIXEL_OBSERVATION_MODEL_HPP
+#define POSE_TRACKING_MODELS_OBSERVATION_MODELS_APPROXIMATE_KINECT_PIXEL_OBSERVATION_MODEL_HPP
+
+#include <cmath>
+#include <tuple>
+
+#include <Eigen/Dense>
+
+#include <fast_filtering/distributions/interfaces/evaluation.hpp>
+#include <fast_filtering/distributions/exponential_distribution.hpp>
+#include <fast_filtering/distributions/uniform_distribution.hpp>
+#include <fast_filtering/distributions/truncated_gaussian.hpp>
+#include <fast_filtering/utils/helper_functions.hpp>
+
+#include <pose_tracking/models/observation_models/kinect_pixel_observation_model.hpp>
+#include <pose_tracking/utils/rigid_body_renderer.hpp>
+#include <pose_tracking/utils/hash_mapping.hpp>
+
+#include <boost/unordered_map.hpp>
+
+namespace ff
+{
+
+/**
+ * \class KinectObservationModel
+ *
+ * \ingroup distributions
+ * \ingroup observation_models
+ */
+template <typename State>
+class ApproximateKinectPixelObservationModel:
+        public GaussianMap<double, Eigen::Matrix<double, 1, 1> >,
+        public Evaluation<double, double>
+{
+public:
+    typedef Eigen::Matrix<double, 1, 1> Noise;
+    typedef double Scalar;
+    typedef double Observation;
+
+    typedef boost::shared_ptr<RigidBodyRenderer> ObjectRendererPtr;
+
+    ApproximateKinectPixelObservationModel(
+            const ObjectRendererPtr object_renderer,
+            const Eigen::Matrix3d& camera_matrix,
+            const size_t n_rows,
+            const size_t n_cols,
+            const Scalar sensor_failure_probability = 0.01,
+            const Scalar object_model_sigma = 0.003,
+            const Scalar sigma_factor = 0.00142478,
+            const Scalar half_life_depth = 1.0,
+            const Scalar max_depth = 6.0,
+            const Scalar min_depth = 0.0,
+            const Scalar approximation_depth = 1.5,
+            const size_t depth_count = 10000,
+            const size_t occlusion_count = 100)
+        : object_renderer_(object_renderer),
+          camera_matrix_(camera_matrix),
+          n_rows_(n_rows),
+          n_cols_(n_cols),
+          predictions_(100), // the size of this vector reflects the number of different
+          occluded_observation_model_(sensor_failure_probability,
+                                     object_model_sigma,
+                                     sigma_factor,
+                                     half_life_depth,
+                                     max_depth),
+          visible_observation_model_(occluded_observation_model_),
+          exponential_distribution_(-log(0.5)/half_life_depth, min_depth),
+          object_model_noise_(0.0, object_model_sigma),
+          sensor_failure_distribution_(min_depth, max_depth),
+          sensor_failure_probability_(sensor_failure_probability),
+          sigma_factor_(sigma_factor),
+          max_depth_(max_depth),
+          min_depth_(min_depth),
+          approximation_depth_(approximation_depth),
+          occlusion_step_(1.0 / double(occlusion_count - 1)),
+          depth_step_((max_depth_ - min_depth_) / double(depth_count - 1))
+    {
+        for(size_t occlusion_index = 0; occlusion_index < occlusion_count; occlusion_index++)
+        {
+            std::vector<double> log_probs(depth_count);
+            for(size_t depth_index = 0; depth_index < depth_count; depth_index++)
+            {
+                Condition(approximation_depth_,
+                          hf::Logit(double(occlusion_index) * occlusion_step_));
+                log_probs[depth_index] = LogProbability(
+                            min_depth_ + double(depth_index) * depth_step_);
+            }
+            samplers_.push_back(hf::DiscreteDistribution(log_probs));
+        }
+    }
+
+
+    virtual ~ApproximateKinectPixelObservationModel() {}
+
+    virtual Observation MapStandardGaussian(const Noise& sample) const
+    {
+        int depth_index = samplers_[occlusion_index_].MapStandardGaussian(sample(0));
+
+        return rendered_depth_ - approximation_depth_ + min_depth_
+                + depth_step_ * double(depth_index);
+    }
+
+    virtual Scalar Probability(const Observation& observation) const
+    {
+        Scalar probability_given_occluded =
+                occluded_observation_model_.Probability(observation);
+
+        Scalar probability_given_visible =
+                visible_observation_model_.Probability(observation);
+
+        return probability_given_occluded * occlusion_probability_ +
+                probability_given_visible * (1.0 - occlusion_probability_);
+    }
+
+    virtual Scalar LogProbability(const Observation& observation) const
+    {
+        return std::log(Probability(observation));
+    }
+
+    virtual void ResetCache()
+    {
+        predictions_.clear();
+    }
+
+    virtual void Condition(const State& state,
+                           const Scalar& occlusion,
+                           size_t index)
+    {
+
+        if (predictions_.find(state) == predictions_.end())
+        {
+            object_renderer_->state(state);
+            object_renderer_->Render(camera_matrix_,
+                                     n_rows_,
+                                     n_cols_,
+                                     predictions_[state]);
+        }
+
+        Condition(predictions_[state][index], occlusion);
+    }
+
+    virtual void Condition(const Scalar& rendered_depth,
+                           const Scalar& occlusion)
+    {
+        rendered_depth_ = rendered_depth;
+        occlusion_probability_ = hf::Sigmoid(occlusion);
+        occlusion_index_ = occlusion_probability_ / occlusion_step_;
+        occluded_observation_model_.Condition(rendered_depth, true);
+        visible_observation_model_.Condition(rendered_depth, false);
+    }
+
+private:
+    ObjectRendererPtr object_renderer_;
+
+    Eigen::Matrix3d camera_matrix_;
+    size_t n_rows_;
+    size_t n_cols_;
+
+    /**
+     * Harbors pairs of (intersect_indices, image_prediction).
+     */
+    boost::unordered_map<Eigen::MatrixXd, std::vector<float> > predictions_;
+
+    KinectPixelObservationModel occluded_observation_model_;
+    KinectPixelObservationModel visible_observation_model_;
+
+    Scalar rendered_depth_;
+    Scalar occlusion_probability_;
+
+    TruncatedGaussian object_model_noise_;
+    UniformDistribution uniform_distribution_;
+    ExponentialDistribution exponential_distribution_;
+    UniformDistribution sensor_failure_distribution_;
+
+    // parameters
+    const Scalar sensor_failure_probability_, sigma_factor_,
+            max_depth_, min_depth_, approximation_depth_;
+
+
+    std::vector<hf::DiscreteDistribution> samplers_;
+
+    double occlusion_step_;
+    double depth_step_;
+    int occlusion_index_;
+};
+
+}
+
+
+
+#endif
diff --git a/src/pose_tracking_interface/trackers/object_tracker.cpp b/src/pose_tracking_interface/trackers/object_tracker.cpp
