betweenFactorMaxMix.h
Go to the documentation of this file.
00001 /*
00002  * betweenFactorMaxMix.h
00003  *
00004  *  Created on: 14.08.2012
00005  *      Author: niko
00006  */
00007 
00008 #ifndef BETWEENFACTORMAXMIX_H_
00009 #define BETWEENFACTORMAXMIX_H_
00010 
00011 #include <gtsam/linear/NoiseModel.h>
00012 #include <Eigen/Eigen>
00013 
00014 namespace vertigo {
00015 
00016   template<class VALUE>
00017   class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2<VALUE, VALUE>
00018   {
00019     public:
00020       BetweenFactorMaxMix() : weight(0.0) {};
00021       BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w)
00022       : gtsam::NoiseModelFactor2<VALUE, VALUE>(model, key1, key2), weight(w), nullHypothesisModel(model2),
00023         betweenFactor(key1, key2, measured, model)  {   };
00024 
00025       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2,
00026           boost::optional<gtsam::Matrix&> H1 = boost::none,
00027           boost::optional<gtsam::Matrix&> H2 =  boost::none) const
00028         {
00029 
00030           // calculate error
00031           gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
00032 
00033 
00034 
00035           // which hypothesis is more likely
00036           double m1 = this->noiseModel_->distance(error);
00037           gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_;
00038           gtsam::Matrix info1(g1->R().transpose()*g1->R());
00039           double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
00040           double l1 = nu1 * exp(-0.5*m1);
00041 
00042           double m2 = nullHypothesisModel->distance(error);
00043           gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
00044           gtsam::Matrix info2(g2->R().transpose()*g2->R());
00045           double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());
00046           double l2 = nu2 * exp(-0.5*m2);
00047 
00048           // if the null hypothesis is more likely, than proceed by applying the weight ...
00049           if (l2>l1) {
00050             if (H1) *H1 = *H1 * weight;
00051             if (H2) *H2 = *H2 * weight;
00052             error *= sqrt(weight);
00053           }
00054 
00055           return error;
00056         };
00057 
00058     private:
00059       gtsam::BetweenFactor<VALUE> betweenFactor;
00060       gtsam::SharedNoiseModel nullHypothesisModel;
00061       double weight;
00062 
00063   };
00064 }
00065 
00066 
00067 #endif /* BETWEENFACTORMAXMIX_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:18