private void makeCSVFromXmpWithCertainFormat(string xmpPath, string statementFormat, CSVFile csv)
{
XMPFile xmp = new XMPFile();
xmp.LoadXML(xmpPath);
XMPGenerator xmpGen = new XMPGenerator();
/*
* Program.AddLog(xmp.GetPosition());
* Program.AddLog("strat add rotation");
* Program.AddLog(xmp.GetRotation());
* Console.ReadLine();*/
//string statement = Path.GetFileNameWithoutExtension(filePaths[2]) + " " + xmp.GetPosition() + " " + xmp.GetRotation();
string xmpPostions = xmp.GetPosition();
string xmpRotations = xmp.GetRotation();
XMPGenerator.FRotator xmpYawPitchAndRoll = xmpGen.GroundVUYawPitchRow(xmpRotations);
string[] Positions = xmpPostions.Split(' ');
string[] Rotations = xmpRotations.Split(' ');
//string statement = string.Format(format_string_images, Path.GetFileNameWithoutExtension(filePaths[i]), Positions[0], Positions[1], Positions[2], Rotations[0], Rotations[1], Rotations[2], Rotations[3], Rotations[4], Rotations[5], Rotations[6], Rotations[7], Rotations[8]);
string statement = string.Format(statementFormat, Path.GetFileNameWithoutExtension(xmpPath), Positions[0], Positions[1], Positions[2], xmpYawPitchAndRoll.Roll, xmpYawPitchAndRoll.Pitch, xmpYawPitchAndRoll.Yaw);
csv.AddLineWithoutIdx(statement);
}
public void SetRelativeXMPFile(int index, string srcFile)
{
isXMPSetting[index] = false;
if (srcFile.Length < 3)
{
Program.AddLog("Set RelativeM Data: wrong filename." + index + " " + srcFile);
return;
}
if (!File.Exists(srcFile))
{
Program.AddLog("Set RelativeM Data: No file." + index + " " + srcFile);
return;
}
string filename = Path.GetFileNameWithoutExtension(srcFile);
GetIndexAndKeyword(out keywords[index], out keyIndex[index], filename);
relativeXMPFilename[index] = srcFile;
if (relativeMtXMPFiles[index] == null)
{
relativeMtXMPFiles[index] = new XMPFile();
}
relativeMtXMPFiles[index].LoadXML(srcFile);
relPostion[index] = GetPostiion(relativeMtXMPFiles[index].GetPosition());
relRotator[index] = GetRotator(relativeMtXMPFiles[index].GetRotation());
isXMPSetting[index] = true;
Program.AddLog("Set RelativeM Data: " + index + " " + srcFile);
}
public void BuildXMPFiles()
{
if (m_images_gps5_list.Count < 1)
{
return;
}
BuildKMLFile();
var nodeFirst = m_images_gps5_list.First();
GPS5DataFile.GPS5Data gpsDataFirst = nodeFirst.Value;
double latRef = gpsDataFirst.latitude;
double lonRef = gpsDataFirst.longitude;
foreach (var node in m_images_gps5_list)
{
GPS5DataFile.GPS5Data gpsData = node.Value;
string filename = node.Key;
string path = Path.GetDirectoryName(filename);
string output_filename = Path.GetFileNameWithoutExtension(filename);
output_filename += ".xmp";
output_filename = Path.Combine(path, output_filename);
XMPFile xmp_file = new XMPFile();
xmp_file.LoadXML("Sample.xmp");
xmp_file.RemoveNode(100);
xmp_file.RemoveNode(102);
xmp_file.RemoveAttribute(1);
// xmp_file.SetGroup();
// xmp_file.SetGPSData(gpsData.latitude, gpsData.longitude, gpsData.altitude);
double posX;
double posY;
//geodeticOffsetInv(latRef, lonRef, gpsData.latitude, gpsData.longitude,out posX,out posY);
posX = KMLFile.distanceEarth(latRef, lonRef, latRef, gpsData.longitude);
if (lonRef > gpsData.longitude)
{
posX *= -1.0;
}
posY = KMLFile.distanceEarth(latRef, lonRef, gpsData.latitude, lonRef);
if (latRef > gpsData.latitude)
{
posY *= -1.0;
}
posX *= 1000.0;
posY *= 1000.0;
xmp_file.SetPosition(posX, posY, gpsData.altitude);
xmp_file.SaveXML(output_filename);
}
return;
}
static void BuildXMPForRotationBias(string[] args)
{
string[] fileEntries = Directory.GetFiles(args[1]);
foreach (string fileName in fileEntries)
{
string ext = Path.GetExtension(fileName);
if (ext == ".xmp")
{
XMPFile xmpFile = new XMPFile();
xmpFile.Load(fileName);
double[] rot = xmpFile.rotation;
double[] new_rot = new double[9];
if (args[2] == "0")
{
//Right hand - 90 right turn
//Left hand - 90 left turn
new_rot[0] = -1.0 * rot[3];
new_rot[1] = -1.0 * rot[4];
new_rot[2] = -1.0 * rot[5];
new_rot[3] = rot[0];
new_rot[4] = rot[1];
new_rot[5] = rot[2];
new_rot[6] = rot[6];
new_rot[7] = rot[7];
new_rot[8] = rot[8];
}
if (args[2] == "1")
{
//Left hand - 90 right turn
//Right hand - 90 left turn
new_rot[0] = rot[3];
new_rot[1] = rot[4];
new_rot[2] = rot[5];
new_rot[3] = -1.0 * rot[0];
new_rot[4] = -1.0 * rot[1];
new_rot[5] = -1.0 * rot[2];
new_rot[6] = rot[6];
new_rot[7] = rot[7];
new_rot[8] = rot[8];
}
xmpFile.WriteRotation(new_rot);
xmpFile.Save();
}
}
}
public void BuildCalibXMPFile()
{
if (!isXMPSetting[4])
{
return;
}
XMPFile save_file = new XMPFile();
save_file.LoadXML(relativeXMPFilename[4]);
foreach (var node in xmpFileDirectory4)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
if (!jfInfo.isXMP)
{
// string posStr1 = jfInfo.xmpFile.GetPosition();
save_file.SetPosePrior("unknown");
save_file.RemoveNode(101);
save_file.RemoveNode(102);
save_file.SaveXML(jfInfo.xmp_filename);
Program.AddLog("SaveCalibXMP: new " + jfInfo.xmp_filename);
continue;
}
if (jfInfo.xmpFile.IsLockedData())
{
continue;
}
//if(jfInfo.xmpFile.IsCalibrationData())
//{
// string posStr1 = jfInfo.xmpFile.GetPosition();
// save_file.SetPosePrior();
// save_file.SetPosition(posStr1);
// save_file.RemoveNode(102);
// save_file.SaveXML(jfInfo.xmp_filename);
// continue;
//}
string posStr = jfInfo.xmpFile.GetPosition();
save_file.CheckPositionNode();
save_file.SetPosePrior("unknown");
save_file.SetPosition(posStr);
save_file.RemoveNode(102);
save_file.SaveXML(jfInfo.xmp_filename);
Program.AddLog("SaveCalibXMP: " + jfInfo.xmp_filename);
}
return;
}
public void Process()
{
foreach (PhotoInfo node in photo_info_list)
{
XMPFile xmp_file = new XMPFile();
xmp_file.LoadXML("Sample.xmp");
double focalLen = focallength;
double ppx_mm = ppx / width - 0.5;
double ppy_mm = ppy / height - 0.5;
xmp_file.SetParameters(focallength, ppx_mm, ppy_mm);
xmp_file.SetPosition(node.x, node.y, node.z);
xmp_file.SetRotation(node.mt);
xmp_file.SetDistortion(distorttion_p1, distorttion_p2, distorttion_p3, distorttion_k1, distorttion_k2);
string filename = Path.GetFileNameWithoutExtension(node.image_path);
filename += ".xmp";
filename = Path.Combine(output_folder, filename);
xmp_file.SaveXML(filename);
}
}
public void BuildXMPFile_LerpGPSData(string imgFolder)
{
if (imgFolder.Length < 3)
{
Program.AddLog("BuildXMPFile_LerpGPSData: wrong Path." + imgFolder);
return;
}
if (!Directory.Exists(imgFolder))
{
Program.AddLog("BuildXMPFile_LerpGPSData: No Path." + imgFolder);
return;
}
if (m_gps_data.Count < 1)
{
return;
}
string[] files = Directory.GetFiles(imgFolder);
GPSData firstGPSData = m_gps_data.First().Value;
foreach (var file in files)
{
string ext = Path.GetExtension(file);
ext = ext.ToLower();
if (ext == ".jpg")
{
string filename = Path.GetFileNameWithoutExtension(file);
string keyword = "";
int idx = 0;
bool isFilename = XMPGenerator.GetIndexAndKeyword(out keyword, out idx, filename);
if (!isFilename)
{
continue;
}
string xmpFileName = Path.ChangeExtension(file, "xmp");
if (File.Exists(xmpFileName))
{
File.Delete(xmpFileName);
}
idx -= 1;
int gpsIdx = idx / 30;
int gpsSecStep = idx % 30;
gpsIdx += 1;
int gpsIdxNext = gpsIdx + 1;
if (!m_gps_data.ContainsKey(gpsIdx))
{
continue;
}
if (!m_gps_data.ContainsKey(gpsIdxNext))
{
continue;
}
GPSData gpsData = m_gps_data[gpsIdx];
GPSData gpsData2 = m_gps_data[gpsIdxNext];
double p = gpsSecStep / 30.0;
double posX = (gpsData2.posX - gpsData.posX) * p + gpsData.posX;
double posY = (gpsData2.posY - gpsData.posY) * p + gpsData.posY;
double altitude = (gpsData2.altitude - gpsData.altitude) * p + gpsData.altitude;
XMPFile xmp_file = new XMPFile();
xmp_file.LoadXML("Sample.xmp");
xmp_file.RemoveNode(100);
xmp_file.RemoveNode(102);
xmp_file.RemoveAttribute(1);
xmp_file.SetPosition(posX, posY, altitude);
xmp_file.SaveXML(xmpFileName);
}
}
}
public void BuildTotalImagesFolder(double gapLimit)
{
if (!Directory.Exists(destFolder))
{
Directory.CreateDirectory(destFolder);
}
//string removeImgFolder = Path.Combine(destFolder, "Remove");
//if (!Directory.Exists(removeImgFolder))
//{
// Directory.CreateDirectory(removeImgFolder);
//}
var gpsFirstNode = m_total_gps_data_list.First();
GPS5DataFile.GPS5Data gpsDataFirst = gpsFirstNode.Value;
double latRef = gpsDataFirst.latitude;
double lonRef = gpsDataFirst.longitude;
double lastPosX = 0;
double lastPosY = 0;
double lastPosZ = 0;
bool gapCheck = false;
foreach (var node in m_total_images)
{
int idx = node.Key;
string imageName = node.Value;
//Caculate XYZ;
GPS5DataFile.GPS5Data gpsData = m_total_gps_data_list[imageName];
double posX;
double posY;
posX = KMLFile.distanceEarth(latRef, lonRef, latRef, gpsData.longitude);
if (lonRef > gpsData.longitude)
{
posX *= -1.0;
}
posY = KMLFile.distanceEarth(latRef, lonRef, gpsData.latitude, lonRef);
if (latRef > gpsData.latitude)
{
posY *= -1.0;
}
// km -> m
posX *= 1000.0;
posY *= 1000.0;
//Check gap limit
if (!gapCheck)
{
lastPosX = posX;
lastPosY = posY;
lastPosZ = gpsData.altitude;
gapCheck = true;
}
else
{
double dist = (posX - lastPosX) * (posX - lastPosX) + (posY - lastPosY) * (posY - lastPosY) + (gpsData.altitude - lastPosZ) * (gpsData.altitude - lastPosZ);
dist = Math.Sqrt(dist);
if (dist < gapLimit)
{
continue;
}
else
{
lastPosX = posX;
lastPosY = posY;
lastPosZ = gpsData.altitude;
}
}
//string filename = Path.GetFileName(imageName);
string newFilename = m_total_new_image_name[imageName];
string destImgFilename = Path.Combine(destFolder, newFilename);
File.Move(imageName, destImgFilename);
Program.AddLog("Move: " + imageName + " To " + destImgFilename);
string xmpFilename = Path.GetFileNameWithoutExtension(newFilename);
xmpFilename += ".xmp";
xmpFilename = Path.Combine(destFolder, xmpFilename);
XMPFile xmp_file = new XMPFile();
xmp_file.LoadXML("Sample.xmp");
xmp_file.RemoveNode(100);
xmp_file.RemoveNode(102);
xmp_file.RemoveAttribute(1);
xmp_file.SetPosition(posX, posY, gpsData.altitude);
xmp_file.SaveXML(xmpFilename);
// Program.AddLog("Save XMP file: " + xmpFilename);
}
}
public void BuildRelXMPFile()
{
if (!isXMPSetting[0])
{
return;
}
if (isXMPSetting[1])
{
int curIdx = 1;
FVector relativePos = new FVector(0, 0, 0);
relativePos.X = relPostion[curIdx].X - relPostion[0].X;
relativePos.Y = relPostion[curIdx].Y - relPostion[0].Y;
relativePos.Z = relPostion[curIdx].Z - relPostion[0].Z;
FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Pitch = relRotator[curIdx].Pitch - relRotator[0].Pitch;
relativeRotator.Yaw = relRotator[curIdx].Yaw - relRotator[0].Yaw;
relativeRotator.Roll = relRotator[curIdx].Roll - relRotator[0].Roll;
XMPFile save_file = new XMPFile();
save_file.LoadXML(relativeXMPFilename[curIdx]);
int key_offset = keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory1)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
FRotator newRot = new FRotator(0, 0, 0);
newRot.Pitch = jfInfo0.rotation.Pitch + relativeRotator.Pitch;
newRot.Yaw = jfInfo0.rotation.Yaw + relativeRotator.Yaw;
newRot.Roll = jfInfo0.rotation.Roll + relativeRotator.Roll;
double[] mt = BuildRotationMatrix(newRot);
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X + relativePos.X;
newPos.Y = jfInfo0.position.Y + relativePos.Y;
newPos.Z = jfInfo0.position.Z + relativePos.Z;
save_file.SetRotation(mt);
save_file.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
}
if (isXMPSetting[2])
{
int curIdx = 2;
FVector relativePos = new FVector(0, 0, 0);
relativePos.X = relPostion[curIdx].X - relPostion[0].X;
relativePos.Y = relPostion[curIdx].Y - relPostion[0].Y;
relativePos.Z = relPostion[curIdx].Z - relPostion[0].Z;
FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Pitch = relRotator[curIdx].Pitch - relRotator[0].Pitch;
relativeRotator.Yaw = relRotator[curIdx].Yaw - relRotator[0].Yaw;
relativeRotator.Roll = relRotator[curIdx].Roll - relRotator[0].Roll;
XMPFile save_file = new XMPFile();
save_file.LoadXML(relativeXMPFilename[curIdx]);
int key_offset = keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory2)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
FRotator newRot = new FRotator(0, 0, 0);
newRot.Pitch = jfInfo0.rotation.Pitch + relativeRotator.Pitch;
newRot.Yaw = jfInfo0.rotation.Yaw + relativeRotator.Yaw;
newRot.Roll = jfInfo0.rotation.Roll + relativeRotator.Roll;
double[] mt = BuildRotationMatrix(newRot);
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X + relativePos.X;
newPos.Y = jfInfo0.position.Y + relativePos.Y;
newPos.Z = jfInfo0.position.Z + relativePos.Z;
save_file.SetRotation(mt);
save_file.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
}
if (isXMPSetting[3])
{
int curIdx = 3;
FVector relativePos = new FVector(0, 0, 0);
relativePos.X = relPostion[curIdx].X - relPostion[0].X;
relativePos.Y = relPostion[curIdx].Y - relPostion[0].Y;
relativePos.Z = relPostion[curIdx].Z - relPostion[0].Z;
FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Pitch = relRotator[curIdx].Pitch - relRotator[0].Pitch;
relativeRotator.Yaw = relRotator[curIdx].Yaw - relRotator[0].Yaw;
relativeRotator.Roll = relRotator[curIdx].Roll - relRotator[0].Roll;
XMPFile save_file = new XMPFile();
save_file.LoadXML(relativeXMPFilename[curIdx]);
int key_offset = keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory3)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
FRotator newRot = new FRotator(0, 0, 0);
newRot.Pitch = jfInfo0.rotation.Pitch + relativeRotator.Pitch;
newRot.Yaw = jfInfo0.rotation.Yaw + relativeRotator.Yaw;
newRot.Roll = jfInfo0.rotation.Roll + relativeRotator.Roll;
double[] mt = BuildRotationMatrix(newRot);
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X + relativePos.X;
newPos.Y = jfInfo0.position.Y + relativePos.Y;
newPos.Z = jfInfo0.position.Z + relativePos.Z;
save_file.SetRotation(mt);
save_file.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
}
}
public void BuildFixedRelXMPFile(string sampleXMPFilename)
{
int curIdx = 1;
FVector relativePos = new FVector(0, 0, 0);
FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Yaw = 90.0f;
XMPFile save_file = new XMPFile();
save_file.LoadXML(sampleXMPFilename);
int key_offset = 0;// keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory1)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
//FRotator newRot = new FRotator(0, 0, 0);
//newRot.Pitch = -jfInfo0.rotation.Roll;
//newRot.Yaw = jfInfo0.rotation.Yaw + 90.0;
//newRot.Roll = 0;
//double[] mt = BuildRotationMatrix(newRot);
double[] mt = GetMtx(jfInfo0.rotationString, 3);
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X;
newPos.Y = jfInfo0.position.Y;
newPos.Z = jfInfo0.position.Z;
save_file.SetRotation(mt);
save_file.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
curIdx = 2;
//FVector relativePos = new FVector(0, 0, 0);
//FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Yaw = -90.0;
XMPFile save_file_2 = new XMPFile();
save_file_2.LoadXML(sampleXMPFilename);
//key_offset = keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory2)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
//FRotator newRot = new FRotator(0, 0, 0);
//newRot.Pitch = jfInfo0.rotation.Roll ;
//newRot.Yaw = jfInfo0.rotation.Yaw - 90.0;
//newRot.Roll = 0;
//double[] mt = BuildRotationMatrix(newRot);
double[] mt = GetMtx(jfInfo0.rotationString, 1);
//double[] mt = jfInfo0.rotation.GetRightDirMtx();
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X;
newPos.Y = jfInfo0.position.Y;
newPos.Z = jfInfo0.position.Z;
save_file_2.SetRotation(mt);
save_file_2.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file_2.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
curIdx = 3;
//FVector relativePos = new FVector(0, 0, 0);
//FRotator relativeRotator = new FRotator(0, 0, 0);
relativeRotator.Yaw = 180;
XMPFile save_file_3 = new XMPFile();
save_file_3.LoadXML(sampleXMPFilename);
//int key_offset = keyIndex[curIdx] - keyIndex[0];
foreach (var node in xmpFileDirectory3)
{
int idx = node.Key;
jpgFileInfo jfInfo = node.Value;
int idx0 = idx - key_offset;
if (!xmpFileDirectory0.ContainsKey(idx0))
{
continue;
}
jpgFileInfo jfInfo0 = xmpFileDirectory0[idx0];
if (!jfInfo0.isXMP)
{
continue;
}
//FRotator newRot = new FRotator(0, 0, 0);
//newRot.Pitch = -jfInfo0.rotation.Pitch;
//newRot.Yaw = jfInfo0.rotation.Yaw + 180;
//newRot.Roll = 0;
double[] mt = GetMtx(jfInfo0.rotationString, 2);
//double[] mt = BuildRotationMatrix(newRot);
FVector newPos = new FVector(0, 0, 0);
newPos.X = jfInfo0.position.X;
newPos.Y = jfInfo0.position.Y;
newPos.Z = jfInfo0.position.Z;
save_file_3.SetRotation(mt);
save_file_3.SetPosition(newPos.X, newPos.Y, newPos.Z);
string saveXMPFilename = Path.ChangeExtension(jfInfo.filename, "xmp");
save_file_3.SaveXML(saveXMPFilename);
Program.AddLog("SaveXML: " + saveXMPFilename);
}
}