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 vgl_intersection(vgl_line_segment_2d<double> lineSegment0, vgl_line_segment_2d<double> lineSegment1, vgl_point_2d<double> &intersectionPoint)
{
vgl_point_3d<double> lineSegment03DP0(lineSegment0.point1().x(), lineSegment0.point1().y(), 0);
vgl_point_3d<double> lineSegment03DP1(lineSegment0.point2().x(), lineSegment0.point2().y(), 0);
vgl_line_segment_3d<double> lineSegment03D(lineSegment03DP0, lineSegment03DP1);
vgl_point_3d<double> lineSegment13DP0(lineSegment1.point1().x(), lineSegment1.point1().y(), 0);
vgl_point_3d<double> lineSegment13DP1(lineSegment1.point2().x(), lineSegment1.point2().y(), 0);
vgl_line_segment_3d<double> lineSegment13D(lineSegment13DP0, lineSegment13DP1);
vgl_point_3d<double> intersectionPoint3D;
bool isIntersect = vgl_intersection(lineSegment03D, lineSegment13D, intersectionPoint3D);
intersectionPoint.set(intersectionPoint3D.x(), intersectionPoint3D.y());
return isIntersect;
}
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());
}
