[REFACTOR] Ran Eclipse Code Formatter on all Added files.
							parent
							
								
									bcb5ca97e0
								
							
						
					
					
						commit
						b387a08b66
					
				|  | @ -43,8 +43,7 @@ public: | |||
|    * Dual Jacobians used to build a dual factor graph. | ||||
|    */ | ||||
|   template<typename FACTOR> | ||||
|   TermsContainer collectDualJacobians(Key key, | ||||
|       const FactorGraph<FACTOR>& graph, | ||||
|   TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph, | ||||
|       const VariableIndex& variableIndex) const { | ||||
|     /*
 | ||||
|      * Iterates through each factor in the factor graph and checks  | ||||
|  | @ -53,43 +52,44 @@ public: | |||
|      */ | ||||
|     TermsContainer Aterms; | ||||
|     if (variableIndex.find(key) != variableIndex.end()) { | ||||
|     for(size_t factorIx: variableIndex[key]) { | ||||
|       typename FACTOR::shared_ptr factor = graph.at(factorIx); | ||||
|       if (!factor->active()) continue; | ||||
|       Matrix Ai = factor->getA(factor->find(key)).transpose(); | ||||
|       Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); | ||||
|       for (size_t factorIx : variableIndex[key]) { | ||||
|         typename FACTOR::shared_ptr factor = graph.at(factorIx); | ||||
|         if (!factor->active()) | ||||
|           continue; | ||||
|         Matrix Ai = factor->getA(factor->find(key)).transpose(); | ||||
|         Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); | ||||
|       } | ||||
|     } | ||||
|     return Aterms; | ||||
|   } | ||||
|   return Aterms; | ||||
| } | ||||
| /**
 | ||||
|  * Identifies active constraints that shouldn't be active anymore. | ||||
|  */ | ||||
| int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, | ||||
|     const VectorValues& lambdas) const; | ||||
|   /**
 | ||||
|    * Identifies active constraints that shouldn't be active anymore. | ||||
|    */ | ||||
|   int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, | ||||
|       const VectorValues& lambdas) const; | ||||
| 
 | ||||
| /**
 | ||||
|  * Builds a dual graph from the current working set. | ||||
|  */ | ||||
| GaussianFactorGraph::shared_ptr buildDualGraph( | ||||
|     const InequalityFactorGraph& workingSet, const VectorValues& delta) const; | ||||
|   /**
 | ||||
|    * Builds a dual graph from the current working set. | ||||
|    */ | ||||
|   GaussianFactorGraph::shared_ptr buildDualGraph( | ||||
|       const InequalityFactorGraph& workingSet, const VectorValues& delta) const; | ||||
| 
 | ||||
| protected: | ||||
| /**
 | ||||
|  * Protected constructor because this class doesn't have any meaning without | ||||
|  * a concrete Programming problem to solve. | ||||
|  */ | ||||
| ActiveSetSolver() : | ||||
|     constrainedKeys_() { | ||||
| } | ||||
|   /**
 | ||||
|    * Protected constructor because this class doesn't have any meaning without | ||||
|    * a concrete Programming problem to solve. | ||||
|    */ | ||||
|   ActiveSetSolver() : | ||||
|       constrainedKeys_() { | ||||
|   } | ||||
| 
 | ||||
| /**
 | ||||
|  * Computes the distance to move from the current point being examined to the next  | ||||
|  * location to be examined by the graph. This should only be used where there are less  | ||||
|  * than two constraints active. | ||||
|  */ | ||||
| boost::tuple<double, int> computeStepSize( | ||||
|     const InequalityFactorGraph& workingSet, const VectorValues& xk, | ||||
|     const VectorValues& p, const double& startAlpha) const; | ||||
|   /**
 | ||||
|    * Computes the distance to move from the current point being examined to the next  | ||||
|    * location to be examined by the graph. This should only be used where there are less  | ||||
|    * than two constraints active. | ||||
|    */ | ||||
|   boost::tuple<double, int> computeStepSize( | ||||
|       const InequalityFactorGraph& workingSet, const VectorValues& xk, | ||||
|       const VectorValues& p, const double& startAlpha) const; | ||||
| }; | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -26,7 +26,7 @@ namespace gtsam { | |||
|  * This class is used to represent an equality constraint on | ||||
|  * a Programming problem of the form Ax = b. | ||||
|  */ | ||||
| class EqualityFactorGraph : public FactorGraph<LinearEquality> { | ||||
| class EqualityFactorGraph: public FactorGraph<LinearEquality> { | ||||
| public: | ||||
|   typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr; | ||||
| 
 | ||||
|  | @ -36,8 +36,8 @@ public: | |||
|    */ | ||||
|   double error(const VectorValues& x) const { | ||||
|     double total_error = 0.; | ||||
|     for(const sharedFactor& factor: *this){ | ||||
|       if(factor) | ||||
|     for (const sharedFactor& factor : *this) { | ||||
|       if (factor) | ||||
|         total_error += factor->error(x); | ||||
|     } | ||||
|     return total_error; | ||||
|  |  | |||
|  | @ -47,10 +47,10 @@ public: | |||
|    * Compute error of a guess. | ||||
|    * Infinity error if it violates an inequality; zero otherwise. */ | ||||
|   double error(const VectorValues& x) const { | ||||
|     for(const sharedFactor& factor: *this) { | ||||
|       if(factor) | ||||
|       if (factor->error(x) > 0) | ||||
|       return std::numeric_limits<double>::infinity(); | ||||
|     for (const sharedFactor& factor : *this) { | ||||
|       if (factor) | ||||
|         if (factor->error(x) > 0) | ||||
|           return std::numeric_limits<double>::infinity(); | ||||
|     } | ||||
|     return 0.0; | ||||
|   } | ||||
|  |  | |||
|  | @ -5,7 +5,6 @@ | |||
|  * @date     1/24/16 | ||||
|  */ | ||||
| 
 | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class InfeasibleOrUnboundedProblem: public ThreadsafeException< | ||||
|  |  | |||
|  | @ -128,7 +128,7 @@ private: | |||
|   /// Collect all terms of a factor into a container.
 | ||||
|   std::vector<std::pair<Key, Matrix> > collectTerms( | ||||
|       const LinearInequality& factor) const { | ||||
|     std::vector<std::pair<Key, Matrix> > terms; | ||||
|     std::vector < std::pair<Key, Matrix> > terms; | ||||
|     for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { | ||||
|       terms.push_back(make_pair(*it, factor.getA(it))); | ||||
|     } | ||||
|  | @ -140,7 +140,7 @@ private: | |||
|       const InequalityFactorGraph& inequalities) const { | ||||
|     InequalityFactorGraph slackInequalities; | ||||
|     for (const auto& factor : lp_.inequalities) { | ||||
|       std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
 | ||||
|       std::vector < std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
 | ||||
|       terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
 | ||||
|       double d = factor->getb()[0]; | ||||
|       slackInequalities.push_back( | ||||
|  |  | |||
|  | @ -109,7 +109,7 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( | |||
|         lp_.cost.end(), std::inserter(difference, difference.end())); | ||||
|     for (Key k : difference) { | ||||
|       size_t dim = keysDim_.at(k); | ||||
|       graph->push_back(JacobianFactor(k, Matrix::Identity(dim,dim), xk.at(k))); | ||||
|       graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); | ||||
|     } | ||||
|   } | ||||
|   return graph; | ||||
|  | @ -136,10 +136,10 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key, | |||
|     const InequalityFactorGraph &workingSet, const VectorValues &delta) const { | ||||
|   // Transpose the A matrix of constrained factors to have the jacobian of the
 | ||||
|   // dual key
 | ||||
|   TermsContainer Aterms = collectDualJacobians<LinearEquality>(key, | ||||
|       lp_.equalities, equalityVariableIndex_); | ||||
|   TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>( | ||||
|       key, workingSet, inequalityVariableIndex_); | ||||
|   TermsContainer Aterms = collectDualJacobians < LinearEquality | ||||
|       > (key, lp_.equalities, equalityVariableIndex_); | ||||
|   TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality | ||||
|       > (key, workingSet, inequalityVariableIndex_); | ||||
|   Aterms.insert(Aterms.end(), AtermsInequalities.begin(), | ||||
|       AtermsInequalities.end()); | ||||
| 
 | ||||
|  | @ -149,7 +149,7 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key, | |||
|     Factor::const_iterator it = lp_.cost.find(key); | ||||
|     if (it != lp_.cost.end()) | ||||
|       b = lp_.cost.getA(it).transpose(); | ||||
|     return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
 | ||||
|     return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
 | ||||
|   } else { | ||||
|     return boost::make_shared<JacobianFactor>(); | ||||
|   } | ||||
|  |  | |||
|  | @ -30,90 +30,89 @@ typedef Eigen::RowVectorXd RowVector; | |||
|  */ | ||||
| class LinearCost: public JacobianFactor { | ||||
| public: | ||||
| 	typedef LinearCost This; ///< Typedef to this class
 | ||||
| 	typedef JacobianFactor Base; ///< Typedef to base class
 | ||||
| 	typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 | ||||
|   typedef LinearCost This; ///< Typedef to this class
 | ||||
|   typedef JacobianFactor Base; ///< Typedef to base class
 | ||||
|   typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 | ||||
| 
 | ||||
| public: | ||||
| 	/** default constructor for I/O */ | ||||
| 	LinearCost() : | ||||
| 			Base() { | ||||
| 	} | ||||
|   /** default constructor for I/O */ | ||||
|   LinearCost() : | ||||
|       Base() { | ||||
|   } | ||||
| 
 | ||||
| 	/** Conversion from HessianFactor */ | ||||
| 	explicit LinearCost(const HessianFactor& hf) { | ||||
| 		throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); | ||||
| 	} | ||||
|   /** Conversion from HessianFactor */ | ||||
|   explicit LinearCost(const HessianFactor& hf) { | ||||
|     throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); | ||||
|   } | ||||
| 
 | ||||
| 	/** Conversion from JacobianFactor */ | ||||
| 	explicit LinearCost(const JacobianFactor& jf) : | ||||
| 			Base(jf) { | ||||
| 		if (jf.isConstrained()) { | ||||
| 			throw std::runtime_error( | ||||
| 					"Cannot convert a constrained JacobianFactor to LinearCost"); | ||||
| 		} | ||||
|   /** Conversion from JacobianFactor */ | ||||
|   explicit LinearCost(const JacobianFactor& jf) : | ||||
|       Base(jf) { | ||||
|     if (jf.isConstrained()) { | ||||
|       throw std::runtime_error( | ||||
|           "Cannot convert a constrained JacobianFactor to LinearCost"); | ||||
|     } | ||||
| 
 | ||||
| 		if (jf.get_model()->dim() != 1) { | ||||
| 			throw std::runtime_error( | ||||
| 					"Only support single-valued linear cost factor!"); | ||||
| 		} | ||||
| 	} | ||||
|     if (jf.get_model()->dim() != 1) { | ||||
|       throw std::runtime_error( | ||||
|           "Only support single-valued linear cost factor!"); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
| 	/** Construct unary factor */ | ||||
| 	LinearCost(Key i1, const RowVector& A1) : | ||||
| 			Base(i1, A1, Vector1::Zero()) { | ||||
| 	} | ||||
|   /** Construct unary factor */ | ||||
|   LinearCost(Key i1, const RowVector& A1) : | ||||
|       Base(i1, A1, Vector1::Zero()) { | ||||
|   } | ||||
| 
 | ||||
| 	/** Construct binary factor */ | ||||
| 	LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, | ||||
| 			double b) : | ||||
| 			Base(i1, A1, i2, A2, Vector1::Zero()) { | ||||
| 	} | ||||
|   /** Construct binary factor */ | ||||
|   LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) : | ||||
|       Base(i1, A1, i2, A2, Vector1::Zero()) { | ||||
|   } | ||||
| 
 | ||||
| 	/** Construct ternary factor */ | ||||
| 	LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, | ||||
| 			const RowVector& A3) : | ||||
| 			Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { | ||||
| 	} | ||||
|   /** Construct ternary factor */ | ||||
|   LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, | ||||
|       const RowVector& A3) : | ||||
|       Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { | ||||
|   } | ||||
| 
 | ||||
| 	/** Construct an n-ary factor
 | ||||
| 	 * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the | ||||
| 	 *         collection of keys and matrices making up the factor. */ | ||||
| 	template<typename TERMS> | ||||
| 	LinearCost(const TERMS& terms) : | ||||
| 			Base(terms, Vector1::Zero()) { | ||||
| 	} | ||||
|   /** Construct an n-ary factor
 | ||||
|    * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the | ||||
|    *         collection of keys and matrices making up the factor. */ | ||||
|   template<typename TERMS> | ||||
|   LinearCost(const TERMS& terms) : | ||||
|       Base(terms, Vector1::Zero()) { | ||||
|   } | ||||
| 
 | ||||
| 	/** Virtual destructor */ | ||||
| 	virtual ~LinearCost() { | ||||
| 	} | ||||
|   /** Virtual destructor */ | ||||
|   virtual ~LinearCost() { | ||||
|   } | ||||
| 
 | ||||
| 	/** equals */ | ||||
| 	virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { | ||||
| 		return Base::equals(lf, tol); | ||||
| 	} | ||||
|   /** equals */ | ||||
|   virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { | ||||
|     return Base::equals(lf, tol); | ||||
|   } | ||||
| 
 | ||||
| 	/** print */ | ||||
| 	virtual void print(const std::string& s = "", | ||||
| 			const KeyFormatter& formatter = DefaultKeyFormatter) const { | ||||
| 		Base::print(s + " LinearCost: ", formatter); | ||||
| 	} | ||||
|   /** print */ | ||||
|   virtual void print(const std::string& s = "", const KeyFormatter& formatter = | ||||
|       DefaultKeyFormatter) const { | ||||
|     Base::print(s + " LinearCost: ", formatter); | ||||
|   } | ||||
| 
 | ||||
| 	/** Clone this LinearCost */ | ||||
| 	virtual GaussianFactor::shared_ptr clone() const { | ||||
| 		return boost::static_pointer_cast<GaussianFactor>( | ||||
| 				boost::make_shared<LinearCost>(*this)); | ||||
| 	} | ||||
|   /** Clone this LinearCost */ | ||||
|   virtual GaussianFactor::shared_ptr clone() const { | ||||
|     return boost::static_pointer_cast < GaussianFactor | ||||
|         > (boost::make_shared < LinearCost > (*this)); | ||||
|   } | ||||
| 
 | ||||
| 	/** Special error_vector for constraints (A*x-b) */ | ||||
| 	Vector error_vector(const VectorValues& c) const { | ||||
| 		return unweighted_error(c); | ||||
| 	} | ||||
|   /** Special error_vector for constraints (A*x-b) */ | ||||
|   Vector error_vector(const VectorValues& c) const { | ||||
|     return unweighted_error(c); | ||||
|   } | ||||
| 
 | ||||
| 	/** Special error for single-valued inequality constraints. */ | ||||
| 	virtual double error(const VectorValues& c) const { | ||||
| 		return error_vector(c)[0]; | ||||
| 	} | ||||
|   /** Special error for single-valued inequality constraints. */ | ||||
|   virtual double error(const VectorValues& c) const { | ||||
|     return error_vector(c)[0]; | ||||
|   } | ||||
| }; | ||||
| // \ LinearCost
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -44,13 +44,14 @@ public: | |||
|   /**
 | ||||
|    * Construct from a constrained noisemodel JacobianFactor with a dual key. | ||||
|    */ | ||||
|   explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey){ | ||||
|   explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : | ||||
|       Base(jf), dualKey_(dualKey) { | ||||
|     if (!jf.isConstrained()) { | ||||
|       throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality"); | ||||
|       throw std::runtime_error( | ||||
|           "Cannot convert an unconstrained JacobianFactor to LinearEquality"); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
|   /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ | ||||
|   explicit LinearEquality(const HessianFactor& hf) { | ||||
|     throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); | ||||
|  | @ -100,15 +101,19 @@ public: | |||
| 
 | ||||
|   /** Clone this LinearEquality */ | ||||
|   virtual GaussianFactor::shared_ptr clone() const { | ||||
|     return boost::static_pointer_cast<GaussianFactor>( | ||||
|         boost::make_shared<LinearEquality>(*this)); | ||||
|     return boost::static_pointer_cast < GaussianFactor | ||||
|         > (boost::make_shared < LinearEquality > (*this)); | ||||
|   } | ||||
| 
 | ||||
|   /// dual key
 | ||||
|   Key dualKey() const { return dualKey_; } | ||||
|   Key dualKey() const { | ||||
|     return dualKey_; | ||||
|   } | ||||
| 
 | ||||
|   /// for active set method: equality constraints are always active
 | ||||
|   bool active() const { return true; } | ||||
|   bool active() const { | ||||
|     return true; | ||||
|   } | ||||
| 
 | ||||
|   /** Special error_vector for constraints (A*x-b) */ | ||||
|   Vector error_vector(const VectorValues& c) const { | ||||
|  | @ -123,11 +128,12 @@ public: | |||
|     return 0.0; | ||||
|   } | ||||
| 
 | ||||
| }; // \ LinearEquality
 | ||||
| 
 | ||||
| }; | ||||
| // \ LinearEquality
 | ||||
| 
 | ||||
| /// traits
 | ||||
| template<> struct traits<LinearEquality> : public Testable<LinearEquality> {}; | ||||
| template<> struct traits<LinearEquality> : public Testable<LinearEquality> { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -52,9 +52,11 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /** Conversion from JacobianFactor */ | ||||
|   explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) { | ||||
|   explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : | ||||
|       Base(jf), dualKey_(dualKey), active_(true) { | ||||
|     if (!jf.isConstrained()) { | ||||
|       throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearInequality"); | ||||
|       throw std::runtime_error( | ||||
|           "Cannot convert an unconstrained JacobianFactor to LinearInequality"); | ||||
|     } | ||||
| 
 | ||||
|     if (jf.get_model()->dim() != 1) { | ||||
|  | @ -64,20 +66,20 @@ public: | |||
| 
 | ||||
|   /** Construct unary factor */ | ||||
|   LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : | ||||
|       Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( | ||||
|           dualKey), active_(true) { | ||||
|       Base(i1, A1, (Vector(1) << b).finished(), | ||||
|           noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { | ||||
|   } | ||||
| 
 | ||||
|   /** Construct binary factor */ | ||||
|   LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, | ||||
|       Key dualKey) : | ||||
|       Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( | ||||
|           dualKey), active_(true) { | ||||
|   LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, | ||||
|       double b, Key dualKey) : | ||||
|       Base(i1, A1, i2, A2, (Vector(1) << b).finished(), | ||||
|           noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { | ||||
|   } | ||||
| 
 | ||||
|   /** Construct ternary factor */ | ||||
|   LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, | ||||
|       const RowVector& A3, double b, Key dualKey) : | ||||
|   LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, | ||||
|       Key i3, const RowVector& A3, double b, Key dualKey) : | ||||
|       Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), | ||||
|           noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { | ||||
|   } | ||||
|  | @ -112,21 +114,29 @@ public: | |||
| 
 | ||||
|   /** Clone this LinearInequality */ | ||||
|   virtual GaussianFactor::shared_ptr clone() const { | ||||
|     return boost::static_pointer_cast<GaussianFactor>( | ||||
|         boost::make_shared<LinearInequality>(*this)); | ||||
|     return boost::static_pointer_cast < GaussianFactor | ||||
|         > (boost::make_shared < LinearInequality > (*this)); | ||||
|   } | ||||
| 
 | ||||
|   /// dual key
 | ||||
|   Key dualKey() const { return dualKey_; } | ||||
|   Key dualKey() const { | ||||
|     return dualKey_; | ||||
|   } | ||||
| 
 | ||||
|   /// return true if this constraint is active
 | ||||
|   bool active() const { return active_; } | ||||
|   bool active() const { | ||||
|     return active_; | ||||
|   } | ||||
| 
 | ||||
|   /// Make this inequality constraint active
 | ||||
|   void activate() { active_ = true; } | ||||
|   void activate() { | ||||
|     active_ = true; | ||||
|   } | ||||
| 
 | ||||
|   /// Make this inequality constraint inactive
 | ||||
|   void inactivate() { active_ = false; } | ||||
|   void inactivate() { | ||||
|     active_ = false; | ||||
|   } | ||||
| 
 | ||||
|   /** Special error_vector for constraints (A*x-b) */ | ||||
|   Vector error_vector(const VectorValues& c) const { | ||||
|  | @ -149,10 +159,12 @@ public: | |||
|     return aTp; | ||||
|   } | ||||
| 
 | ||||
| }; // \ LinearInequality
 | ||||
| }; | ||||
| // \ LinearInequality
 | ||||
| 
 | ||||
| /// traits
 | ||||
| template<> struct traits<LinearInequality> : public Testable<LinearInequality> {}; | ||||
| template<> struct traits<LinearInequality> : public Testable<LinearInequality> { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -42,7 +42,8 @@ struct QP { | |||
|   QP(const GaussianFactorGraph& _cost, | ||||
|       const EqualityFactorGraph& _linearEqualities, | ||||
|       const InequalityFactorGraph& _linearInequalities) : | ||||
|       cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) { | ||||
|       cost(_cost), equalities(_linearEqualities), inequalities( | ||||
|           _linearInequalities) { | ||||
|   } | ||||
| 
 | ||||
|   /** print */ | ||||
|  |  | |||
|  | @ -36,9 +36,7 @@ private: | |||
| 
 | ||||
| public: | ||||
|   RawQP() : | ||||
|       row_to_constraint_v(), E(), IG(), IL(), varNumber(1), | ||||
|       b(), g(), varname_to_key(), H(), f(), | ||||
|       obj_name(), name_(), up(), lo(), Free() { | ||||
|       row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { | ||||
|   } | ||||
| 
 | ||||
|   void setName( | ||||
|  |  | |||
|  | @ -28,20 +28,20 @@ using namespace std; | |||
| using namespace gtsam; | ||||
| using namespace boost::assign; | ||||
| 
 | ||||
| GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) | ||||
| GTSAM_CONCEPT_TESTABLE_INST (LinearEquality) | ||||
| 
 | ||||
| namespace { | ||||
|   namespace simple { | ||||
|     // Terms we'll use
 | ||||
|     const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> > | ||||
|       (make_pair(5, Matrix3::Identity())) | ||||
|       (make_pair(10, 2*Matrix3::Identity())) | ||||
|       (make_pair(15, 3*Matrix3::Identity())); | ||||
| namespace simple { | ||||
| // Terms we'll use
 | ||||
| const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix> | ||||
|     > (make_pair(5, Matrix3::Identity()))( | ||||
|         make_pair(10, 2 * Matrix3::Identity()))( | ||||
|         make_pair(15, 3 * Matrix3::Identity())); | ||||
| 
 | ||||
|     // RHS and sigmas
 | ||||
|     const Vector b = (Vector(3) << 1., 2., 3.).finished(); | ||||
|     const SharedDiagonal noise = noiseModel::Constrained::All(3); | ||||
|   } | ||||
| // RHS and sigmas
 | ||||
| const Vector b = (Vector(3) << 1., 2., 3.).finished(); | ||||
| const SharedDiagonal noise = noiseModel::Constrained::All(3); | ||||
| } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors) | |||
|   { | ||||
|     // One term constructor
 | ||||
|     LinearEquality expected( | ||||
|       boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); | ||||
|         boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); | ||||
|     LinearEquality actual(terms[0].first, terms[0].second, b, 0); | ||||
|     EXPECT(assert_equal(expected, actual)); | ||||
|     LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); | ||||
|  | @ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors) | |||
|   { | ||||
|     // Two term constructor
 | ||||
|     LinearEquality expected( | ||||
|       boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); | ||||
|         boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); | ||||
|     LinearEquality actual(terms[0].first, terms[0].second, | ||||
|       terms[1].first, terms[1].second, b, 0); | ||||
|         terms[1].first, terms[1].second, b, 0); | ||||
|     EXPECT(assert_equal(expected, actual)); | ||||
|     LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); | ||||
|     EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); | ||||
|  | @ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors) | |||
|   { | ||||
|     // Three term constructor
 | ||||
|     LinearEquality expected( | ||||
|       boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); | ||||
|         boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); | ||||
|     LinearEquality actual(terms[0].first, terms[0].second, | ||||
|       terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); | ||||
|         terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); | ||||
|     EXPECT(assert_equal(expected, actual)); | ||||
|     LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); | ||||
|     EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); | ||||
|  | @ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors) | |||
| /* ************************************************************************* */ | ||||
| TEST(LinearEquality, Hessian_conversion) { | ||||
|   HessianFactor hessian(0, (Matrix(4,4) << | ||||
|         1.57,        2.695,         -1.1,        -2.35, | ||||
|        2.695,      11.3125,        -0.65,      -10.225, | ||||
|         -1.1,        -0.65,            1,          0.5, | ||||
|        -2.35,      -10.225,          0.5,         9.25).finished(), | ||||
|           1.57, 2.695, -1.1, -2.35, | ||||
|           2.695, 11.3125, -0.65, -10.225, | ||||
|           -1.1, -0.65, 1, 0.5, | ||||
|           -2.35, -10.225, 0.5, 9.25).finished(), | ||||
|       (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), | ||||
|       73.1725); | ||||
| 
 | ||||
|  | @ -169,8 +169,8 @@ TEST(LinearEquality, matrices) | |||
|   augmentedJacobianExpected << jacobianExpected, rhsExpected; | ||||
| 
 | ||||
|   Matrix augmentedHessianExpected = | ||||
|     augmentedJacobianExpected.transpose() * simple::noise->R().transpose() | ||||
|     * simple::noise->R() * augmentedJacobianExpected; | ||||
|   augmentedJacobianExpected.transpose() * simple::noise->R().transpose() | ||||
|   * simple::noise->R() * augmentedJacobianExpected; | ||||
| 
 | ||||
|   // Whitened Jacobian
 | ||||
|   EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); | ||||
|  | @ -210,8 +210,8 @@ TEST(LinearEquality, operators ) | |||
|   // test gradient at zero
 | ||||
|   Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); | ||||
|   VectorValues expectedG; | ||||
|   expectedG.insert(1, (Vector(2) <<  0.2, -0.1).finished()); | ||||
|   expectedG.insert(2, (Vector(2) << -0.2,  0.1).finished()); | ||||
|   expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); | ||||
|   expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); | ||||
|   VectorValues actualG = lf.gradientAtZero(); | ||||
|   EXPECT(assert_equal(expectedG, actualG)); | ||||
| } | ||||
|  | @ -233,5 +233,8 @@ TEST(LinearEquality, empty ) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -42,8 +42,8 @@ QP createTestCase() { | |||
|   //TODO:  THIS TEST MIGHT BE WRONG : the last parameter  might be 5 instead of 10 because the form of the equation
 | ||||
|   // Should be 0.5x'Gx + gx + f : Nocedal 449
 | ||||
|   qp.cost.push_back( | ||||
|       HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, | ||||
|           3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0)); | ||||
|       HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, | ||||
|           Z_1x1, 10.0)); | ||||
| 
 | ||||
|   // Inequality constraints
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
 | ||||
|  | @ -96,8 +96,8 @@ QP createEqualityConstrainedTest() { | |||
|   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
 | ||||
|   // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
 | ||||
|   qp.cost.push_back( | ||||
|       HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, | ||||
|           2.0 * I_1x1, Z_1x1, 0.0)); | ||||
|       HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, | ||||
|           0.0)); | ||||
| 
 | ||||
|   // Equality constraints
 | ||||
|   // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
 | ||||
|  | @ -140,9 +140,9 @@ TEST(QPSolver, indentifyActiveConstraints) { | |||
|       qp.inequalities, currentSolution); | ||||
| 
 | ||||
|   CHECK(!workingSet.at(0)->active()); // inactive
 | ||||
|   CHECK(workingSet.at(1)->active()); // active
 | ||||
|   CHECK(workingSet.at(2)->active()); // active
 | ||||
|   CHECK(!workingSet.at(3)->active()); // inactive
 | ||||
|   CHECK(workingSet.at(1)->active());// active
 | ||||
|   CHECK(workingSet.at(2)->active());// active
 | ||||
|   CHECK(!workingSet.at(3)->active());// inactive
 | ||||
| 
 | ||||
|   VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); | ||||
|   VectorValues expectedSolution; | ||||
|  | @ -211,14 +211,15 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { | |||
|   expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); | ||||
|   CHECK(assert_equal(expectedSolution, solution, 1e-100)); | ||||
| } | ||||
| 
 | ||||
| pair<QP, QP> testParser(QPSParser parser) { | ||||
|   QP exampleqp = parser.Parse(); | ||||
|   QP expectedqp; | ||||
|   Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); | ||||
|   // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
 | ||||
|   expectedqp.cost.push_back( | ||||
|       HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, | ||||
|           10.0 * I_1x1, -2.0 * kOne, 4.0)); | ||||
|       HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, 10.0 * I_1x1, | ||||
|           -2.0 * kOne, 4.0)); | ||||
|   // 2x + y >= 2
 | ||||
|   // -x + 2y <= 6
 | ||||
|   expectedqp.inequalities.push_back( | ||||
|  | @ -267,13 +268,15 @@ QP createTestMatlabQPEx() { | |||
|   //        0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
 | ||||
|   // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
 | ||||
|   qp.cost.push_back( | ||||
|       HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, | ||||
|           2.0 * I_1x1, 6 * I_1x1, 1000.0)); | ||||
|       HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, | ||||
|           6 * I_1x1, 1000.0)); | ||||
| 
 | ||||
|   // Inequality constraints
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
 | ||||
|   qp.inequalities.push_back( | ||||
|       LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
 | ||||
|   qp.inequalities.push_back( | ||||
|       LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1      <= 0
 | ||||
|   qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); //      -x2 <= 0
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue