void show (const int iter, const float err, const float* val) {
bar.clear();
bar.addSegment(GnuplotPoint2(*val, -5), GnuplotPoint2(*val, +5));
gp.draw(plot);
gp.flush();
usleep(1000*250);
}
template <typename Func> Plot(Func func, float min = -10, float max = +10) {
plot.add(&fu);
plot.add(&bar);
float stepSize = (max-min) / 300;
for (float x = min; x <= max; x+=stepSize) {
float y = func(&x);
fu.add(GnuplotPoint2(x,y));
}
}
TEST(FundamentalMatrix, estimate) {
Garden desc;
//IDIS desc;
//Metal1 desc;
//House3D desc;
StereoPlot plot(desc);
StereoReconstruction sr(desc);
//sr.perform();
for (int i = 0; i < 8; ++i) {
const Eigen::Vector3d pil = toVec(desc.fm.getPointLeft(i));
const Eigen::Vector3d pir = toVec(desc.fm.getPointRight(i));
const float res = (pir.transpose() * (desc.fm.getFundamentalMatrix() * pil));
std::cout << "#" << res << std::endl;
//ASSERT_GE(0.1, std::abs(res));
}
plot.rPoints.add(GnuplotPoint2( desc.fm.getEpipoleRight()(0), desc.fm.getEpipoleRight()(1) ));
plot.lPoints.add(GnuplotPoint2( desc.fm.getEpipoleLeft()(0), desc.fm.getEpipoleLeft()(1) ));
for (int i = 0; i < desc.fm.getNumCorrespondences(); ++i) {
// const int i = 1;
const Eigen::Vector3d piL = toVec(desc.fm.getPointLeft(i));
const Eigen::Vector3d piR = toVec(desc.fm.getPointRight(i));
const Eigen::Vector3d lR = desc.fm.getEpilineRight(piL);
const Eigen::Vector3d lL = desc.fm.getEpilineLeft(piR);
plot.lPoints.add(GnuplotPoint2(piL(0), piL(1)));
plot.rPoints.add(GnuplotPoint2(piR(0), piR(1)));
for (int xx = -400; xx < desc.imgLeft.getWidth()*3; xx+=25) {
int x1 = xx;
int x2 = xx + 25;
{
float y1 = -(lL(0)*x1 + lL(2)) / lL(1);
float y2 = -(lL(0)*x2 + lL(2)) / lL(1);
plot.lLines.addSegment(GnuplotPoint2(x1,y1), GnuplotPoint2(x2,y2));
}
{
float y1 = -(lR(0)*x1 + lR(2)) / lR(1);
float y2 = -(lR(0)*x2 + lR(2)) / lR(1);
plot.rLines.addSegment(GnuplotPoint2(x1,y1), GnuplotPoint2(x2,y2));
}
}
//}
}
ImageChannel test(desc.imgRight.getWidth(), desc.imgRight.getHeight());
CV::Transform::affine(desc.fm.getRectificationRight(), desc.imgRight, test);
ImageFactory::writeJPEG("/tmp/1.jpg", test);
plot.draw();
sleep(1000);
}
