public Vec3f GetPath()
{
int imageNumber = 0;
SlamImageAnalyzer slam = new SlamImageAnalyzer();
while (true)
{
//Mat rgb;
//Mat depth;
// rgb = Cv2.ImRead($"../../image_map/rgb_{imageNumber:D5}.jpg");
// depth = Cv2.ImRead($"../../image_map/depth_{imageNumber:D5}.png");
//Mat tcw = slam.GetTcw(rgb);
//Vec3f pos = slam.GetPos(rgb);
//Console.WriteLine($"{pos.Item0}, {pos.Item1}, {pos.Item2}");
//bool ok;
//imageNumber++;
//return pos;
}
}
public Point3D getvalue(int ImageNumber, int InputPixelX, int InPutPixelY)
{
instance = this;
Point3D pt = new Point3D();
SlamImageAnalyzer slam = new SlamImageAnalyzer();
Mat Matrix4Cam3D1 = new Mat(4, 1, MatType.CV_32F);
//Matrix4Cam3D1.Set<float>(0, 0, x); //world x
//Matrix4Cam3D1.Set<float>(1, 0, z); //world y
//Matrix4Cam3D1.Set<float>(2, 0, y); //world z
//Matrix4Cam3D1.Set<float>(3, 0, 1); // homogenius coordinate 1
float rgbX1 = 0;
float rgbY1 = 0;
Mat MatrixUV1 = new Mat(4, 1, MatType.CV_32F);
MatrixUV1.Set <float>(0, 0, 0);
MatrixUV1.Set <float>(1, 0, 0);
int imCols; //image width
int imRows; //image height
float depthMaxLimit = 6; // 6 meter
float eps = (float)0.001;
int inputX1 = InputPixelX;
int inputY1 = InPutPixelY;
float D1 = 0;
float dd1 = 0;
Mat rgb;
Mat depth;
string rgbpath = System.IO.Path.Combine(Global.ImageDataPath, "rgb_" + ImageNumber.ToString("D5") + ".jpg");
rgb = Cv2.ImRead(rgbpath);
string depthpath = System.IO.Path.Combine(Global.ImageDataPath, "depth_" + ImageNumber.ToString("D5") + ".png");
//rgb = Cv2.ImRead($"../../image_map/rgb_{ImageNumber:D5}.jpg");
depth = Cv2.ImRead(depthpath);
float depthvalue = slam.GetDepth(depth, new Point(InputPixelX, InPutPixelY));
float depthgood = checkDepthValue(depthvalue, (int)depthMaxLimit * 1000);
Size imSize = rgb.Size();
Mat tcw = slam.GetTcw(rgb);
Vec3f pos = slam.GetPos(rgb);
// size of tcw
Size sizeOfTcw = tcw.Size();
if (depthgood == 0 || depthgood == -1)
{
VIewer3D.Instance.RemoveSomething();
}
else
{
Mat Matrix4W1 = new Mat(4, 1, MatType.CV_32F);
for (int i = 0; i < 4; i++)
{
Matrix4W1.Set <float>(i, 0, 0);
}
// RGB->get x, y, z world coordinate
Matrix4W1 = estimateWorldCoordinateFromCameraCoordinate(tcw, imSize, inputX1, inputY1, depthgood, eps);//TODO: Input depth value
if (!Matrix4W1.Empty())
{
float mTx = Matrix4W1.At <float>(0, 0);
float mTy = Matrix4W1.At <float>(1, 0);
float mTz = Matrix4W1.At <float>(2, 0);
pt = new Point3D(mTx, mTy, mTz);
}
// Console.WriteLine(depthgood);
// Console.WriteLine($"{pos.Item0}, {pos.Item1}, {pos.Item2}" + "\n" + depthgood);
}
return(pt);
}
