8000 Update ModelAligner to handle GPS and custom coords. and more by ferreram · Pull Request #1371 · colmap/colmap · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Update ModelAligner to handle GPS and custom coords. and more #1371

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 11 commits into from
Jan 23, 2022
Merged

Update ModelAligner to handle GPS and custom coords. and more #1371

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
4f7409f
Adding GPS conversions to and from ENU coords. (GPS -> ENU, ECEF -> E…
IfremerUnderwater Jan 7, 2022
8daf338
Update of ModelAligner function. Now handles either GPS or custom co…
IfremerUnderwater Jan 7, 2022
d66d2ba
Adding const & const ref.
IfremerUnderwater Jan 23, 2022
5223791
Removing begining / trailing spaces.
IfremerUnderwater Jan 23, 2022
1e094b1
Ensure that first image exists when using merge_origins option.
IfremerUnderwater Jan 23, 2022
61887ed
Using Eigen isApprox function in the BOOST_CHECK functions instead of…
IfremerUnderwater Jan 23, 2022
8383c5e
Explicitly give the ref/origin lat/lon coordinates when converting fr…
IfremerUnderwater Jan 23, 2022
6729874
Add a ConvertCameraLocations function to apply the common conversion …
IfremerUnderwater Jan 23, 2022
c8e7ac3
Directly iterate over the containers in the range-based for loops in …
IfremerUnderwater Jan 23, 2022
11b4a48
Merge branch 'dev' into model_aligner_handle_gps_in_txt_file
IfremerUnderwater Jan 23, 2022
43da879
Adding a const and turning output function parameters to pointers.
IfremerUnderwater Jan 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
72 changes: 72 additions & 0 deletions src/base/gps.cc
View file
Open in desktop
8000
Original file line number Diff line number Diff line change
Expand Up @@ -120,4 +120,76 @@ std::vector<Eigen::Vector3d> GPSTransform::XYZToEll(
return ell;
}

std::vector<Eigen::Vector3d> GPSTransform::EllToENU(
const std::vector<Eigen::Vector3d>& ell, const double lat0,
const double lon0) const {
// Convert GPS (lat / lon / alt) to ECEF
std::vector<Eigen::Vector3d> xyz = EllToXYZ(ell);

return XYZToENU(xyz, lat0, lon0);
}

std::vector<Eigen::Vector3d> GPSTransform::XYZToENU(
const std::vector<Eigen::Vector3d>& xyz, const double lat0,
const double lon0) const {
std::vector<Eigen::Vector3d> enu(xyz.size());

// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU

// ECEF to ENU Rot :
const double cos_lat0 = std::cos(DegToRad(lat0));
const double sin_lat0 = std::sin(DegToRad(lat0));

const double cos_lon0 = std::cos(DegToRad(lon0));
const double sin_lon0 = std::sin(DegToRad(lon0));

Eigen::Matrix3d R;
R << -sin_lon0, cos_lon0, 0., -sin_lat0 * cos_lon0, -sin_lat0 * sin_lon0,
cos_lat0, cos_lat0 * cos_lon0, cos_lat0 * sin_lon0, sin_lat0;

// Convert ECEF to ENU coords. (w.r.t. ECEF ref == xyz[0])
for (size_t i = 0; i < xyz.size(); ++i) {
enu[i] = R * (xyz[i] - xyz[0]);
}

return enu;
}

std::vector<Eigen::Vector3d> GPSTransform::ENUToEll(
const std::vector<Eigen::Vector3d>& enu, const double lat0,
const double lon0, const double alt0) const {
return XYZToEll(ENUToXYZ(enu, lat0, lon0, alt0));
}

std::vector<Eigen::Vector3d> GPSTransform::ENUToXYZ(
const std::vector<Eigen::Vector3d>& enu, const double lat0,
const double lon0, const double alt0) const {
std::vector<Eigen::Vector3d> xyz(enu.size());

// ECEF ref (origin)
const Eigen::Vector3d xyz_ref =
EllToXYZ({Eigen::Vector3d(lat0, lon0, alt0)})[0];

// ENU to ECEF Rot :
const double cos_lat0 = std::cos(DegToRad(lat0));
const double sin_lat0 = std::sin(DegToRad(lat0));

const double cos_lon0 = std::cos(DegToRad(lon0));
const double sin_lon0 = std::sin(DegToRad(lon0));

Eigen::Matrix3d R;
R << -sin_lon0, cos_lon0, 0., -sin_lat0 * cos_lon0, -sin_lat0 * sin_lon0,
cos_lat0, cos_lat0 * cos_lon0, cos_lat0 * sin_lon0, sin_lat0;

// R is ECEF to ENU so Transpose to get inverse
R.transposeInPlace();

// Convert ENU to ECEF coords.
for (size_t i = 0; i < enu.size(); ++i) {
xyz[i] = (R * enu[i]) + xyz_ref;
}

return xyz;
}

} // namespace colmap
18 changes: 18 additions & 0 deletions src/base/gps.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,24 @@ class GPSTransform {
std::vector<Eigen::Vector3d> XYZToEll(
const std::vector<Eigen::Vector3d>& xyz) const;

// Convert GPS (lat / lon / alt) to ENU coords. with lat0 and lon0
// defining the origin of the ENU frame
std::vector<Eigen::Vector3d> EllToENU(const std::vector<Eigen::Vector3d>& ell,
const double lat0,
const double lon0) const;

std::vector<Eigen::Vector3d> XYZToENU(const std::vector<Eigen::Vector3d>& xyz,
const double lat0,
const double lon0) const;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Seems like this should be a private method?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't really know here. It's not used yet anywhere else yet but might become handy for later use / applications? Also this will give me some troubles for the test functions ^^


std::vector<Eigen::Vector3d> ENUToEll(const std::vector<Eigen::Vector3d>& enu,
const double lat0, const double lon0,
const double alt0) const;

std::vector<Eigen::Vector3d> ENUToXYZ(const std::vector<Eigen::Vector3d>& enu,
const double lat0, const double lon0,
const double alt0) const;

private:
// Semimajor axis.
double a_;
Expand Down
146 changes: 128 additions & 18 deletions src/base/gps_test.cc
View file
Open in desktop
1E79
Original file line number Diff line number Diff line change
Expand Up @@ -53,9 +53,7 @@ BOOST_AUTO_TEST_CASE(TestEllToXYZGRS80) {
const auto xyz = gps_tform.EllToXYZ(ell);

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(std::abs(xyz[i](0) - ref_xyz[i](0)) < 1e-8);
BOOST_CHECK(std::abs(xyz[i](1) - ref_xyz[i](1)) < 1e-8);
BOOST_CHECK(std::abs(xyz[i](2) - ref_xyz[i](2)) < 1e-8);
BOOST_CHECK(xyz[i].isApprox(ref_xyz[i], 1e-8));
}
}

Expand All @@ -76,9 +74,7 @@ BOOST_AUTO_TEST_CASE(TestEllToXYZWGS84) {
const auto xyz = gps_tform.EllToXYZ(ell);

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(std::abs(xyz[i](0) - ref_xyz[i](0)) < 1e-8);
BOOST_CHECK(std::abs(xyz[i](1) - ref_xyz[i](1)) < 1e-8);
BOOST_CHECK(std::abs(xyz[i](2) - ref_xyz[i](2)) < 1e-8);
BOOST_CHECK(xyz[i].isApprox(ref_xyz[i], 1e-8));
}
}

Expand All @@ -99,9 +95,7 @@ BOOST_AUTO_TEST_CASE(TestXYZToEll_GRS80) {
const auto ell = gps_tform.XYZToEll(xyz);

for (size_t i = 0; i < xyz.size(); ++i) {
BOOST_CHECK(std::abs(ell[i](0) - ref_ell[i](0)) < 1e-5);
BOOST_CHECK(std::abs(ell[i](1) - ref_ell[i](1)) < 1e-5);
BOOST_CHECK(std::abs(ell[i](2) - ref_ell[i](2)) < 1e-5);
BOOST_CHECK(ell[i].isApprox(ref_ell[i], 1e-5));
}
}

Expand All @@ -122,9 +116,7 @@ BOOST_AUTO_TEST_CASE(TestXYZToEll_WGS84) {
const auto ell = gps_tform.XYZToEll(xyz);

for (size_t i = 0; i < xyz.size(); ++i) {
BOOST_CHECK(std::abs(ell[i](0) - ref_ell[i](0)) < 1e-5);
BOOST_CHECK(std::abs(ell[i](1) - ref_ell[i](1)) < 1e-5);
BOOST_CHECK(std::abs(ell[i](2) - ref_ell[i](2)) < 1e-5);
BOOST_CHECK(ell[i].isApprox(ref_ell[i], 1e-5));
}
}

Expand All @@ -141,9 +133,7 @@ BOOST_AUTO_TEST_CASE(TestXYZToEllToXYZ_GRS80) {
const auto xyz2 = gps_tform.EllToXYZ(ell);

for (size_t i = 0; i < xyz.size(); ++i) {
BOOST_CHECK(std::abs(xyz[i](0) - xyz2[i](0)) < 1e-5);
BOOST_CHECK(std::abs(xyz[i](1) - xyz2[i](1)) < 1e-5);
BOOST_CHECK(std::abs(xyz[i](2) - xyz2[i](2)) < 1e-5);
BOOST_CHECK(xyz[i].isApprox(xyz2[i], 1e-5));
}
}

Expand All @@ -160,8 +150,128 @@ BOOST_AUTO_TEST_CASE(TestXYZToEllToXYZ_WGS84) {
const auto xyz2 = gps_tform.EllToXYZ(ell);

for (size_t i = 0; i < xyz.size(); ++i) {
BOOST_CHECK(std::abs(xyz[i](0) - xyz2[i](0)) < 1e-5);
BOOST_CHECK(std::abs(xyz[i](1) - xyz2[i](1)) < 1e-5);
BOOST_CHECK(std::abs(xyz[i](2) - xyz2[i](2)) < 1e-5);
BOOST_CHECK(xyz[i].isApprox(xyz2[i], 1e-5));
}
}

BOOST_AUTO_TEST_CASE(TestEllToENUWGS84) {
std::vector<Eigen::Vector3d> ell;
ell.emplace_back(48 + 8. / 60 + 51.70361 / 3600,
11 + 34. / 60 + 10.51777 / 3600, 561.1851);
ell.emplace_back(48 + 8. / 60 + 52.40575 / 3600,
11 + 34. / 60 + 11.77179 / 3600, 561.1509);
std::vector<Eigen::Vector3d> ref_xyz;
ref_xyz.emplace_back(4.177239709042750e6, 0.855153779923415e6,
4.728267404769168e6);
ref_xyz.emplace_back(4.177218660452103e6, 0.855175931344048e6,
4.728281850382507e6);

GPSTransform gps_tform(GPSTransform::WGS84);

// Get lat0, lon0 origin from ref
const auto ori_ell = gps_tform.XYZToEll({ref_xyz[0]})[0];

// Get ENU ref from ECEF ref
const auto ref_enu = gps_tform.XYZToENU(ref_xyz, ori_ell(0), ori_ell(1));

// Get ENU from Ell
const auto enu = gps_tform.EllToENU(ell, ori_ell(0), ori_ell(1));

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(enu[i].isApprox(ref_enu[i], 1e-8));
}
}

BOOST_AUTO_TEST_CASE(TestXYZToENU) {
std::vector<Eigen::Vector3d> ell;
ell.emplace_back(48 + 8. / 60 + 51.70361 / 3600,
11 + 34. / 60 + 10.51777 / 3600, 561.1851);
ell.emplace_back(48 + 8. / 60 + 52.40575 / 3600,
11 + 34. / 60 + 11.77179 / 3600, 561.1509);
std::vector<Eigen::Vector3d> ref_xyz;
ref_xyz.emplace_back(4.177239709042750e6, 0.855153779923415e6,
4.728267404769168e6);
ref_xyz.emplace_back(4.177218660452103e6, 0.855175931344048e6,
4.728281850382507e6);

GPSTransform gps_tform(GPSTransform::WGS84);

const auto xyz = gps_tform.EllToXYZ(ell);

// Get lat0, lon0 origin from ref
const auto ori_ell = gps_tform.XYZToEll({ref_xyz[0]})[0];

// Get ENU from ECEF ref
const auto ref_enu = gps_tform.XYZToENU(ref_xyz, ori_ell(0), ori_ell(1));

// Get ENU from ECEF
const auto enu = gps_tform.XYZToENU(xyz, ori_ell(0), ori_ell(1));

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(enu[i].isApprox(ref_enu[i], 1e-8));
}
}

BOOST_AUTO_TEST_CASE(TestENUToEllWGS84) {
std::vector<Eigen::Vector3d> ref_ell;
ref_ell.emplace_back(48 + 8. / 60 + 51.70361 / 3600,
11 + 34. / 60 + 10.51777 / 3600, 561.1851);
ref_ell.emplace_back(48 + 8. / 60 + 52.40575 / 3600,
11 + 34. / 60 + 11.77179 / 3600, 561.1509);

std::vector<Eigen::Vector3d> xyz;
xyz.emplace_back(4.177239709042750e6, 0.855153779923415e6,
4.728267404769168e6);
xyz.emplace_back(4.177218660452103e6, 0.855175931344048e6,
4.728281850382507e6);

GPSTransform gps_tform(GPSTransform::WGS84);

// Get lat0, lon0 origin from ref
const auto ori_ell = gps_tform.XYZToEll(xyz);
const double lat0 = ori_ell[0](0);
const double lon0 = ori_ell[0](1);
const double alt0 = ori_ell[0](2);

// Get ENU from ECEF
const auto enu = gps_tform.XYZToENU(xyz, lat0, lon0);

const auto xyz_enu = gps_tform.ENUToXYZ(enu, lat0, lon0, alt0);

// Get Ell from ENU
const auto ell = gps_tform.ENUToEll(enu, lat0, lon0, alt0);

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(ell[i].isApprox(ref_ell[i], 1e-5));
}
}

BOOST_AUTO_TEST_CASE(TestENUToXYZ) {
std::vector<Eigen::Vector3d> ell;
ell.emplace_back(48 + 8. / 60 + 51.70361 / 3600,
11 + 34. / 60 + 10.51777 / 3600, 561.1851);
ell.emplace_back(48 + 8. / 60 + 52.40575 / 3600,
11 + 34. / 60 + 11.77179 / 3600, 561.1509);
std::vector<Eigen::Vector3d> ref_xyz;
ref_xyz.emplace_back(4.177239709042750e6, 0.855153779923415e6,
4.728267404769168e6);
ref_xyz.emplace_back(4.177218660452103e6, 0.855175931344048e6,
4.728281850382507e6);

GPSTransform gps_tform(GPSTransform::WGS84);

// Get lat0, lon0 origin from Ell
const double lat0 = ell[0](0);
const double lon0 = ell[0](1);
const double alt0 = ell[0](2);

// Get ENU from Ell
const auto enu = gps_tform.EllToENU(ell, lat0, lon0);

// Get XYZ from ENU
const auto xyz = gps_tform.ENUToXYZ(enu, lat0, lon0, alt0);

for (size_t i = 0; i < ell.size(); ++i) {
BOOST_CHECK(xyz[i].isApprox(ref_xyz[i], 1e-8));
}
}
Loading
0