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;
}
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);
}