Beispiel #1
0
cv::Mat griddata(const std::vector<cv::Point2f> &xy,const std::vector<float> &v,
                 int left,int top,int width,int height,float defaultValue) {
    assert(width>0 && height>0);
    BEGIN_TIME;
    cv::Mat grid = cv::Mat::ones(height,width,CV_32F)*defaultValue;
    std::vector<cv::Vec3i> tris = trianglate(xy);
    for(uint i=0;i<tris.size();++i) {
        const cv::Vec3i &tri = tris[i];
        fill_tri(grid,tri,xy,v,left,top);
    }
    PRINT_TIME("griddata");
    return grid;
}
Beispiel #2
0
TEST(Estimator,Triangulate){
    auto estimator=GSLAM::Estimator::create();
    if(!estimator.get())
    {
        LOG(WARNING)<<"Test aborded since estimator not created.";
        return ;
    }

    GSLAM::SE3 ref2cur(GSLAM::SO3::exp(GSLAM::Point3d(0.1,0.1,0.1)),
                       GSLAM::Point3d(1,1,0));
    GSLAM::Point3d refDirection(0,0,1);
    GSLAM::Point3d ptGround=refDirection*svar.GetDouble("Estimator.Triangulate.Depth",5.);
    GSLAM::Point3d curDirection=(ref2cur*ptGround);
    curDirection=curDirection/curDirection.z;

    GSLAM::Point3d refPtEst;
    estimator->trianglate(ref2cur,refDirection,curDirection,refPtEst);
    EXPECT_LE((refPtEst-ptGround).norm(),1e-6);
}