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++; } }
bool VglPlus::isLeftSide(const vgl_line_segment_2d<double> & seg, const vgl_point_2d<double> & p) { vgl_point_2d<double> b = seg.point1(); vgl_point_2d<double> a = seg.point2(); return ((b.x() - a.x())*(p.y() - a.y()) - (b.y() - a.y())*(p.x() - a.x())) > 0; }
void VilBaplSIFT::set_location(bapl_keypoint_sptr & keypoint, const vgl_point_2d<double> & p) { bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoint.as_pointer()); sift->set_location_i(p.x()); sift->set_location_j(p.y()); }