void Frame::setCamParams(const CamParams &c)
{
// monocular
cam = c;
iproj.setZero();
iproj(0,0) = c.fx;
iproj(1,1) = c.fy;
iproj(0,2)=c.cx;
iproj(1,2)=c.cy;
iproj(2,2)=1.0;
// stereo
disp_to_cart(0,0) = 1/c.fx;
disp_to_cart(1,1) = 1/c.fy;
disp_to_cart(0,3) = -c.cx/c.fx;
disp_to_cart(1,3) = -c.cy/c.fy;
disp_to_cart(2,3) = 1.0;
disp_to_cart(3,2) = 1/(c.tx*c.fx);
cart_to_disp.setZero();
cart_to_disp(0,0) = c.fx;
cart_to_disp(1,1) = c.fy;
cart_to_disp(0,2) = c.cx;
cart_to_disp(1,2) = c.cy;
cart_to_disp(2,3) = c.fx*c.tx;
cart_to_disp(3,2) = 1.0;
if (c.tx > 0.0)
isStereo = true;
else
isStereo = false;
}
I operator()(I begin, S end, R rel = R{}, P proj_ = P{}) const
{
auto &&irel = invokable(rel);
auto &&iproj = invokable(proj_);
auto i = begin;
if(begin != end)
{
while(++i != end)
{
if(irel(iproj(*i), iproj(*begin)))
return i;
begin = i;
}
}
return i;
}
std::pair<I, O> operator()(I begin, S end_, O out, P proj = P{}) const
{
auto &&iproj = invokable(proj);
I i = next_to(begin, end_), end = i;
while(begin != i)
*--out = iproj(*--i);
return {end, out};
}
bool
operator()(I first, S last, F pred, P proj = P{}) const
{
auto &&ipred = as_function(pred);
auto &&iproj = as_function(proj);
for(; first != last; ++first)
if(ipred(iproj(*first)))
return true;
return false;
}
bool
operator()(I first, S last, F pred, P proj = P{}) const
{
auto &&ipred = as_function(pred);
auto &&iproj = as_function(proj);
for(; first != last; ++first)
if(!ipred(iproj(*first)))
break;
return first == last;
}
