void f(vnl_vector<double> const &x, vnl_vector<double> &fx)
{
double f = x[0];
double p1 = x[1];
double p2 = x[2];
double p3 = x[3];
double u = pp_.x();
double v = pp_.y();
vnl_matrix<double> K(3, 3, 0);
K[0][0] = K[1][1] = f;
K[0][2] = u;
K[1][2] = v;
K[2][2] = 1.0;
// dual image of the absolute conic
vnl_matrix<double> diac = K * K.transpose();
vnl_matrix<double> pVec(3, 1, 0);
pVec[0][0] = p1;
pVec[1][0] = p2;
pVec[2][0] = p3;
int idx = 0;
for (int i = 0; i<projections_.size(); i++) {
vnl_matrix<double> P = projections_[i].as_matrix();
vnl_matrix<double> A = P.extract(3, 3, 0, 0);
vnl_matrix<double> a = P.extract(3, 1, 0, 3);
vnl_matrix<double> tmp = A - a * pVec.transpose();
vnl_matrix<double> KKt_right = tmp * diac * tmp.transpose();
KKt_right /= KKt_right[2][2]; // normalize
// constraint in up triangle
fx[idx] = diac[0][0] - KKt_right[0][0];
idx++;
fx[idx] = diac[1][1] - KKt_right[1][1];
idx++;
fx[idx] = diac[0][1] - KKt_right[0][1];
idx++;
fx[idx] = diac[0][2] - KKt_right[0][2];
idx++;
fx[idx] = diac[1][2] - KKt_right[1][2];
idx++;
}
}
void PContextSourceState_t::Color(const pDomain &cdom)
{
Color(cdom, PDPoint(pVec(1)));
}
void PContextSourceState_t::Color(const pVec &color, const float alpha)
{
Color(PDPoint(color), PDPoint(pVec(alpha)));
}
void PContextSourceState_t::Color(const float red, const float green, const float blue, const float alpha)
{
Color(pVec(red, green, blue), alpha);
}
SEXP nvVecEmpty(int size){
host_vector<T> *vec = new host_vector<T>(size);
Rcpp::XPtr<host_vector<T> > pVec(vec);
return pVec;
}
void nvVecToSEXP(SEXP mat, T* x){
Rcpp::XPtr<host_vector<T> > pVec(mat);
thrust::copy(pVec->getPtr()->begin(), pVec->getPtr()->end(), x);
}
SEXP sexpToHostVector(T *x, int size){
host_vector<T> *vec = new host_vector<T>(x, size);
Rcpp::XPtr<host_vector<T> > pVec(vec);
return pVec;
}
