quadratic_program.h 7.62 KB
Newer Older
1 2 3 4 5 6
#pragma once

#include <Eigen/SVD>
#include <Array.hh>
#include <QuadProg++.hh>

7 8 9
#include <core/rational_function.h>
#include <core/vertical_segment.h>

10 11
class quadratic_program
{
12 13
	public:
		//! \brief Constructor need to specify the number of coefficients
14 15
        quadratic_program(int np, int nq, bool compute_delta = false) :
        _np(np), _nq(nq), _compute_delta(compute_delta), CI(0.0, _np+_nq, 0)
16
        { }
17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77

		//! \brief Remove the already defined constraints
		void clear_constraints()
		{
			CI.resize(_np+_nq, 0);
		}

		//! \brief Add a constraint by specifying the vector
		void add_constraints(const vec& c)
		{
			const int m = CI.nrows();
			const int n = CI.ncols();

			if(n > 0)
			{
				// Construct temp buffer
				double* temp = new double[n*m];
				for(int u=0; u<n; ++u)
				{
					for(int v=0; v<m; ++v)
					{
						temp[u*m + v] = CI[v][u];
					}
				}

				// Resize matrix CI
				CI.resize(m, n+1);

				// Recopy data
				for(int u=0; u<n+1; ++u)
				{
					if(u==n)
					{
						for(int v=0; v<m; ++v)
							CI[v][u] = c[v];
					}
					else
					{
						for(int v=0; v<m; ++v)
							CI[v][u] = temp[u*m + v];
					}
				}
				delete[] temp;
			}
			else
			{
				// Resize matrix CI
				CI.resize(m, 1);

				// Recopy data
				for(int u=0; u<m; ++u)
					CI[n][u] = c[u];
			}
		}

		//! \brief Provide the number of constraints
		int nb_constraints() const
		{
			return CI.ncols();
		}

78 79 80 81 82 83
        //! Set the indices of the remaining data
        void set_training_set(const std::list<unsigned int>& ts)
        {
            this->training_set = ts;
        }

84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
		//! \brief Solves the quadratic program and update the p and 
		//! q vector if necessary.
		inline bool solve_program(QuadProgPP::Vector<double>& x, double& delta, vec& p, vec& q)
		{
			bool solves_qp = solve_program(x, delta) ;

			if(solves_qp)
			{
				double norm = 0.0;
				for(int i=0; i<_np+_nq; ++i)
				{
					const double v = x[i];
					norm += v*v ;
					if(i < _np)
					{
						p[i] = v ;
					}
					else
					{
						q[i-_np] = v ;
					}
				}
				return norm > 0.0;
			}
			else
			{
				return false ;
			}
		}

		//! \brief Solves the quadratic program
		inline bool solve_program(QuadProgPP::Vector<double>& v, double& delta)
		{        
			const int m = CI.nrows();
			const int n = CI.ncols();

			QuadProgPP::Matrix<double> G (0.0, m, m) ;
			QuadProgPP::Vector<double> g (0.0, m) ;
			QuadProgPP::Vector<double> ci(0.0, n) ;
			QuadProgPP::Matrix<double> CE(0.0, m, 0) ;
			QuadProgPP::Vector<double> ce(0.0, 0) ;
125 126 127 128 129 130 131 132 133 134 135

			if(_compute_delta)
			{
				// Update the ci column with the delta parameter
				// (See Celis et al. 2007 p.12)
				Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::HouseholderQRPreconditioner> svd(Eigen::MatrixXd::Map(&CI[0][0], m, n));
				const double sigma_m = svd.singularValues()(std::min(m, n)-1) ;
				const double sigma_M = svd.singularValues()(0) ;
				delta = sigma_M / sigma_m ;
			}

136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
			// Select the size of the result vector to
			// be equal to the dimension of p + q
			for(int i=0; i<m; ++i)
			{
				G[i][i] = 1.0 ;
			}

			// Each constraint (fitting interval or point
			// add another dimension to the constraint
			// matrix
			for(int i=0; i<n; ++i)
			{
				// Norm of the row vector
				double norm = 0.0 ;

				for(int j=0; j<m; ++j)
				{
					norm += CI[j][i]*CI[j][i] ; ;
				}

				// Set the c vector, will later be updated using the
				// delta parameter.
				ci[i] = -delta * sqrt(norm) ;
			}

			// Compute the solution
			const double cost = QuadProgPP::solve_quadprog(G, g, CE, ce, CI, ci, v);

			bool solves_qp = !(cost == std::numeric_limits<double>::infinity());
			return solves_qp;
		}

168 169
#define PACANOWSKI2012

170 171
        //! \brief Test all the constraints of the data.
        //! Add the sample that is farest away from the function.
172
		bool test_constraints(int ny, const rational_function_1d* r, const ptr<vertical_segment>& data)
173
		{
174 175
#ifdef PACANOWSKI2012
            int nb_failed = 0;
176 177
            double max_dev = 0.0; // Maximum absolute distance of the current
                                  // solution to the data.
178
            std::list<unsigned int>::iterator max_ind;
179 180
            vec cu, cl;

181 182
            std::list<unsigned int>::iterator it;
            for(it = training_set.begin(); it != training_set.end(); it++)
183
			{
184 185
                vec x, yl, yu;
                data->get(*it, x, yl, yu);
186 187

				vec y = r->value(x);
188 189
				bool fail_upper = y[ny] > yu[ny];
				bool fail_lower = y[ny] < yl[ny];
190 191
				if(fail_lower || fail_upper)
				{
192
                    const double dev = std::abs(0.5*(yu[ny]+yl[ny]) - y[ny]);
193

194
					nb_failed++;
195

196 197 198 199
                    if(max_dev < dev)
                    {
                        get_constraint(x, yl, yu, ny, r, cu, cl);
                        max_dev = dev;
200
                        max_ind = it;
201
                    }
202 203
				}
			}
Laurent Belcour's avatar
DONE:  
Laurent Belcour committed
204
#ifdef DEBUG
205 206
            std::cout << "<<TRACE>> " << nb_failed << " constraints where not satified." << std::endl;
            std::cout << "<<TRACE>> an interval failed the test with distance = " << max_dev << std::endl;
Laurent Belcour's avatar
DONE:  
Laurent Belcour committed
207
#endif
208 209 210 211 212

            if(nb_failed > 0)
            {
                add_constraints(cu);
                add_constraints(cl);
213 214 215 216
                training_set.erase(max_ind);
#ifdef DEBUG
                std::cout << "<<DEBUG>> number of remaining training elements: " << training_set.size() << std::endl;
#endif
217 218 219 220 221 222 223

                return false;
            }
            else
            {
                return true;
            }
224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243
#else
            int n = next_unmatching_constraint(0, ny, r, data);
            if(n < data->size())
            {
                vec x, yl, yu;
                data->get(n, x, yl, yu);

                vec cu, cl;
                get_constraint(x, yl, yu, ny, r, cu, cl);

                add_constraints(cu);
                add_constraints(cl);

                return false;
            }
            else
            {
                return true;
            }
#endif
244 245 246 247 248
		}

		//! \brief Generate two constraint vectors from a vertical segment and a
		//! ration function type.
		inline void get_constraint(const vec& xi, const vec& yl, const vec& yu, int ny,
249 250
		                           const rational_function_1d* func,
		                           vec& cu, vec& cl);
251 252 253

		//! \brief Give the next position in the data that is not satisfied.
		//! This method works only for a single color channel ny !
254 255 256 257 258
		static int next_unmatching_constraint(int i, int ny, const rational_function_1d* r,
		                                      const vertical_segment* data);

	protected:
		int _np, _nq;
259
		bool _compute_delta;
260
		QuadProgPP::Matrix<double> CI;
261 262 263 264

        //! Contains the indices of the vertical segment unused during the
        //! rational interpolation.
        std::list<unsigned int> training_set;
265 266 267 268 269 270 271 272 273 274 275 276 277 278 279
};
		

inline void quadratic_program::get_constraint(const vec& xi, const vec& yl, const vec& yu, 
                                              int ny, const rational_function_1d* func,
															 vec& cu, vec& cl)
{
	cu.resize(_np+_nq);
	cl.resize(_np+_nq);

	// Create two vector of constraints
	for(int j=0; j<_np+_nq; ++j)
	{
		// Filling the p part
		if(j<_np)
280
		{
281 282 283
			const double pi = func->p(xi, j) ;
			cu[j] =  pi ;
			cl[j] = -pi ;
284 285

		}
286 287 288 289
		// Filling the q part
		else
		{
			const double qi = func->q(xi, j-_np) ;
290

291 292 293 294 295
			cu[j] = -yu[ny] * qi ;
			cl[j] =  yl[ny] * qi ;
		}
	}
}
296

297 298 299 300 301 302 303
int quadratic_program::next_unmatching_constraint(int i, int ny, const rational_function_1d* r,
                                                  const vertical_segment* data)
{
	for(int n=i; n<data->size(); ++n)
	{
		vec x, yl, yu;
		data->get(n, x, yl, yu);
304

305
		vec y = r->value(x);
306
        if(y[ny] < yl[ny] || y[ny] > yu[ny])
307 308 309 310 311 312
		{
			return n;
		}
	}
	return data->size();
}