void disparityFitPlane(cv::InputArray disparity, cv::InputArray image, cv::OutputArray dest, int slicRegionSize, float slicRegularization, float slicMinRegionRatio, int slicMaxIteration, int ransacNumofSample, float ransacThreshold)
{
//disparityFitTest(ransacNumofSample, ransacThreshold);
//cv::FileStorage pointxml("planePoint.xml", cv::FileStorage::WRITE); int err = 0;
Mat segment;
SLIC(image, segment, slicRegionSize, slicRegularization, slicMinRegionRatio, slicMaxIteration);
vector<vector<Point3f>> points;
SLICSegment2Vector3D_<float>(segment, disparity, 0, points);
Mat disp32f = Mat::zeros(dest.size(), CV_32F);
for (int i = 0; i < points.size(); ++i)
{
if (points[i].size() < 3)
{
if (!points[i].empty())
{
for (int j = 0; j < points[i].size(); ++j)
{
points[i][j].z = 0.f;
}
}
}
else
{
Point3f abc;
fitPlaneRANSAC(points[i], abc, ransacNumofSample, ransacThreshold, 1);
//for refinement(if nessesary)
int v = countArrowablePointDistanceZ(points[i], abc, ransacThreshold);
/*double rate = (double)v / points[i].size() * 100;
int itermax = 1;
for (int n = 0; n < itermax;n++)
{
if (rate < 30)
{
//pointxml <<format("point%03d",err++)<< points[i];
fitPlaneRANSAC(points[i], abc, ransacNumofSample, ransacThreshold, 1);
v = countArrowablePointDistanceZ(points[i], abc, ransacThreshold);
rate = (double)v / points[i].size() * 100;
}
}*/
for (int j = 0; j < points[i].size(); ++j)
{
points[i][j].z = points[i][j].x*abc.x + points[i][j].y*abc.y + abc.z;
}
}
}
SLICVector3D2Signal(points, image.size(), disp32f);
if (disparity.depth() == CV_32F)
{
disp32f.copyTo(dest);
}
else if (disparity.depth() == CV_8U || disparity.depth() == CV_16U || disparity.depth() == CV_16S || disparity.depth() == CV_32S)
{
disp32f.convertTo(dest, disparity.type(), 1.0, 0.5);
}
else
{
disp32f.convertTo(dest, disparity.type());
}
}
