Skip to content

Commit

Permalink
Export Poses: Added support of landmarks. Fixed g2o export with 6DoF …
Browse files Browse the repository at this point in the history 
…landmark constaints. Adjusted g2o export landmark id. (#1199)
  • Loading branch information
@matlabbe
matlabbe committed Jan 20, 2024
1 parent 856e372 commit 13cd5e7
Show file tree
Hide file tree
Showing 3 changed files with 211 additions and 57 deletions.
195 changes: 159 additions & 36 deletions corelib/src/optimizer/OptimizerG2O.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2054,7 +2054,43 @@ bool OptimizerG2O::saveGraph(
q.w());
}

int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0;
// For landmarks, determinate which one has observation with orientation
std::map<int, bool> isLandmarkWithRotation;
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int landmarkId = iter->second.from() < 0?iter->second.from():iter->second.to() < 0?iter->second.to():0;
if(landmarkId != 0 && isLandmarkWithRotation.find(landmarkId) == isLandmarkWithRotation.end())
{
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
UDEBUG("Tag %d has no orientation", landmarkId);
}
else
{
isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
UDEBUG("Tag %d has orientation", landmarkId);
}
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
isLandmarkWithRotation.insert(std::make_pair(landmarkId, false));
UDEBUG("Tag %d has no orientation", landmarkId);
}
else
{
isLandmarkWithRotation.insert(std::make_pair(landmarkId, true));
UDEBUG("Tag %d has orientation", landmarkId);
}

}
}

int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first:0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if (isSlam2d())
Expand All @@ -2063,18 +2099,30 @@ bool OptimizerG2O::saveGraph(
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
landmarkOffset-iter->first,
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
}
else if(!landmarksIgnored())
{
// VERTEX_XY id x y
fprintf(file, "VERTEX_XY %d %f %f\n",
iter->first,
iter->second.x(),
iter->second.y());
if(uValue(isLandmarkWithRotation, iter->first, false))
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
}
else
{
// VERTEX_XY id x y
fprintf(file, "VERTEX_XY %d %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y());
}
}
}
else
Expand All @@ -2095,12 +2143,29 @@ bool OptimizerG2O::saveGraph(
}
else if(!landmarksIgnored())
{
// VERTEX_TRACKXYZ id x y z
fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z());
if(uValue(isLandmarkWithRotation, iter->first, false))
{
// VERTEX_SE3 id x y z qw qx qy qz
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
}
else
{
// VERTEX_TRACKXYZ id x y z
fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z());
}
}
}
}
Expand All @@ -2116,32 +2181,90 @@ bool OptimizerG2O::saveGraph(
}
if(isSlam2d())
{
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
if(uValue(isLandmarkWithRotation, iter->first, false))
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().theta(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5));
}
else
{
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
}
}
else
{
// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET,
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
if(uValue(isLandmarkWithRotation, iter->first, false))
{
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
q.x(),
q.y(),
q.z(),
q.w(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(0, 3),
iter->second.infMatrix().at<double>(0, 4),
iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(1, 3),
iter->second.infMatrix().at<double>(1, 4),
iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(2, 2),
iter->second.infMatrix().at<double>(2, 3),
iter->second.infMatrix().at<double>(2, 4),
iter->second.infMatrix().at<double>(2, 5),
iter->second.infMatrix().at<double>(3, 3),
iter->second.infMatrix().at<double>(3, 4),
iter->second.infMatrix().at<double>(3, 5),
iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5));
}
else
{
// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET,
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
}
}
continue;
}
Expand Down
45 changes: 31 additions & 14 deletions guilib/src/DatabaseViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2753,32 +2753,49 @@ void DatabaseViewer::exportPoses(int format)
}
}

if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
{
UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
std::map<int, Transform>::iterator iter=poses.begin();
while(iter!=poses.end() && iter->first < 0)
UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
{
poses.erase(iter++);
}
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.from() < 0 || iter->second.to() < 0)
{
links.erase(iter++);
}
else
{
++iter;
}
}
}

std::map<int, double> stamps;
if(format == 1 || format == 10 || format == 11)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
if(iter->first<0 && format == 11) // in case of landmarks
{
stamps.insert(std::make_pair(iter->first, stamp));
stamps.insert(std::make_pair(iter->first, 0));
}
else
{
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
{
stamps.insert(std::make_pair(iter->first, stamp));
}
}
}
if(stamps.size()!=poses.size())
Expand Down
28 changes: 21 additions & 7 deletions guilib/src/MainWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6269,8 +6269,8 @@ void MainWindow::exportPoses(int format)
std::multimap<int, Link> links;
if(localTransforms.empty())
{
poses = std::map<int, Transform>(_currentPosesMap.lower_bound(1), _currentPosesMap.end());
links = std::multimap<int, Link>(_currentLinksMap.lower_bound(1), _currentLinksMap.end());
poses = std::map<int, Transform>(_currentPosesMap.begin(), _currentPosesMap.end());
links = std::multimap<int, Link>(_currentLinksMap.begin(), _currentLinksMap.end());
}
else
{
Expand All @@ -6291,22 +6291,36 @@ void MainWindow::exportPoses(int format)
}
}

if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
{
UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
std::map<int, Transform>::iterator iter=poses.begin();
while(iter!=poses.end() && iter->first < 0)
UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
{
poses.erase(iter++);
}
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.from() < 0 || iter->second.to() < 0)
{
links.erase(iter++);
}
else
{
++iter;
}
}
}

std::map<int, double> stamps;
if(format == 1 || format == 10 || format == 11)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(_cachedSignatures.contains(iter->first))
if(iter->first < 0 && format == 11) // in case of landmarks
{
stamps.insert(std::make_pair(iter->first, 0));
}
else if(_cachedSignatures.contains(iter->first))
{
stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
}
Expand Down

0 comments on commit 13cd5e7

Please sign in to comment.