i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added GPS class for convenience, database viewer can view GPS values and export to KML format

Browse Source
This commit is contained in:
matlabbe
2017-09-26 14:13:06 -04:00
parent 8759fda632
commit 9691a4f361
35 changed files with 1150 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/DBDriver.cpp
View File

@@ -713,7 +713,7 @@ bool DBDriver::getNodeInfo(
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity,
std::vector<double> & gps) const
GPS & gps) const
{
bool found = false;
// look in the trash

23
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1756,7 +1756,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity,
std::vector<double> & gps) const
GPS & gps) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -1854,12 +1854,13 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
gps.resize(6,0);
std::vector<double> gpsV(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
if((unsigned int)dataSize == gpsV.size()*sizeof(double) && data)
{
memcpy(gps.data(), data, dataSize);
memcpy(gpsV.data(), data, dataSize);
gps = GPS(gpsV[0], gpsV[1], gpsV[2], gpsV[3], gpsV[4], gpsV[5]);
}
}
}
@@ -2395,7 +2396,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
if(gps.size() == 6)
{
s->sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
}
s->setSaved(true);
nodes.push_back(s);
@@ -4320,6 +4321,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
}
}
std::vector<double> gps;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
// ignore user_data
@@ -4345,14 +4347,21 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
if(s->sensorData().gps().empty())
if(s->sensorData().gps().stamp() <= 0.0)
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().gps().data(), s->sensorData().gps().size()*sizeof(double), SQLITE_STATIC);
gps.resize(6,0.0);
gps[0] = s->sensorData().gps().stamp();
gps[1] = s->sensorData().gps().longitude();
gps[2] = s->sensorData().gps().latitude();
gps[3] = s->sensorData().gps().altitude();
gps[4] = s->sensorData().gps().error();
gps[5] = s->sensorData().gps().bearing();
rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}

2
corelib/src/DBDriverSqlite3.h
View File

@@ -127,7 +127,7 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;

5
corelib/src/DBReader.cpp
View File

@@ -268,7 +268,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
int mapId;
Transform localTransform, pose, groundTruth;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
if(previousStamp && stamp && stamp > previousStamp)
{
@@ -323,7 +323,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
double stamp;
Transform groundTruth;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
@@ -437,6 +437,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.setId(seq);
data.setStamp(stamp);
data.setGroundTruth(groundTruth);
data.setGPS(gps);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,

85
corelib/src/GeodeticCoords.cpp
View File

@@ -51,8 +51,24 @@ namespace rtabmap {
inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
inline double RAD2DEG(const double x) { return x*180.0/M_PI;}
inline double square(const double & value) {return value*value;}
GeodeticCoords::GeodeticCoords() :
latitude_(0.0),
longitude_(0.0),
altitude_(0.0)
{
}
GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
latitude_(latitude),
longitude_(longitude),
altitude_(altitude)
{
}
//*---------------------------------------------------------------
// geodeticToGeocentric_WGS84
// ---------------------------------------------------------------*/
@@ -123,19 +139,72 @@ cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
return out;
}
GeodeticCoords::GeodeticCoords() :
latitude_(0.0),
longitude_(0.0),
altitude_(0.0)
void GeodeticCoords::fromGeocentric_WGS84(const cv::Point3d& geocentric)
{
static const double a = 6378137; // Semi-major axis of the Earth (meters)
static const double b = 6356752.3142; // Semi-minor axis:
const double sa2 = a*a;
const double sb2 = b*b;
const double e2 = (sa2 - sb2) / sa2;
const double ep2 = (sa2 - sb2) / sb2;
const double p = std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y);
const double theta = atan2(geocentric.z * a, p * b);
longitude_ = atan2(geocentric.y, geocentric.x);
latitude_ = atan2(
geocentric.z + ep2 * b * sin(theta) * sin(theta) * sin(theta),
p - e2 * a * cos(theta) * cos(theta) * cos(theta));
const double clat = cos(latitude_);
const double slat = sin(latitude_);
const double N = sa2 / std::sqrt(sa2 * clat * clat + sb2 * slat * slat);
altitude_ = p / clat - N;
longitude_ = RAD2DEG(longitude_);
latitude_ = RAD2DEG(latitude_);
}
GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
latitude_(latitude),
longitude_(longitude),
altitude_(altitude)
void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
{
fromGeocentric_WGS84(ENU_WGS84ToGeocentric_WGS84(enu, origin));
}
cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
{
// Generate reference 3D point:
cv::Point3f originGeocentric;
originGeocentric = origin.toGeocentric_WGS84();
cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
// Z axis -> In direction out-ward the center of the Earth:
cv::Vec3d REF_X, REF_Y, REF_Z;
REF_Z = cv::normalize(P_ref);
// 1st column: Starting at the reference point, move in the tangent
// direction
// east-ward: I compute this as the derivative of P_ref wrt "longitude":
// A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1]
// A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0]
// A_east[2] = 0; --> 0
// ---------------------------------------------------------------------------
cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
REF_X = cv::normalize(AUX_X);
// 2nd column: The cross product:
REF_Y = REF_Z.cross(REF_X);
cv::Point3d out_coords;
out_coords.x =
REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
out_coords.y =
REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
out_coords.z =
REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;
return out_coords;
}
}

99
corelib/src/Graph.cpp
View File

@@ -425,6 +425,105 @@ bool importPoses(
return false;
}
bool exportGPS(
const std::string & filePath,
const std::map<int, GPS> & gpsValues,
unsigned int rgba)
{
UDEBUG("%s", filePath.c_str());
std::string tmpPath = filePath;
std::string ext = UFile::getExtension(filePath);
if(ext.compare("kml")!=0 && ext.compare("txt")!=0)
{
UERROR("Only txt and kml formats are supported!");
return false;
}
FILE* fout = 0;
#ifdef _MSC_VER
fopen_s(&fout, tmpPath.c_str(), "w");
#else
fout = fopen(tmpPath.c_str(), "w");
#endif
if(fout)
{
if(ext.compare("kml")==0)
{
std::string values;
for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
{
values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
}
// switch argb (Qt format) -> abgr
unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
std::string colorHexa = uFormat("%08x", abgr);
fprintf(fout, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
fprintf(fout, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
fprintf(fout, "<Document>\n"
" <name>%s</name>\n", tmpPath.c_str());
fprintf(fout, " <StyleMap id=\"msn_ylw-pushpin\">\n"
" <Pair>\n"
" <key>normal</key>\n"
" <styleUrl>#sn_ylw-pushpin</styleUrl>\n"
" </Pair>\n"
" <Pair>\n"
" <key>highlight</key>\n"
" <styleUrl>#sh_ylw-pushpin</styleUrl>\n"
" </Pair>\n"
" </StyleMap>\n"
" <Style id=\"sh_ylw-pushpin\">\n"
" <IconStyle>\n"
" <scale>1.2</scale>\n"
" </IconStyle>\n"
" <LineStyle>\n"
" <color>%s</color>\n"
" </LineStyle>\n"
" </Style>\n"
" <Style id=\"sn_ylw-pushpin\">\n"
" <LineStyle>\n"
" <color>%s</color>\n"
" </LineStyle>\n"
" </Style>\n", colorHexa.c_str(), colorHexa.c_str());
fprintf(fout, " <Placemark>\n"
" <name>%s</name>\n"
" <styleUrl>#msn_ylw-pushpin</styleUrl>"
" <LineString>\n"
" <coordinates>\n"
" %s\n"
" </coordinates>\n"
" </LineString>\n"
" </Placemark>\n"
"</Document>\n"
"</kml>\n",
uSplit(tmpPath, '.').front().c_str(),
values.c_str());
}
else
{
fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
{
fprintf(fout, "%f %f %f %f %f %f\n",
iter->second.stamp(),
iter->second.longitude(),
iter->second.latitude(),
iter->second.altitude(),
iter->second.error(),
iter->second.bearing());
}
}
fclose(fout);
return true;
}
return false;
}
// KITTI evaluation
float lengths[] = {100,200,300,400,500,600,700,800};
int32_t num_lengths = 8;

46
corelib/src/Memory.cpp
View File

@@ -1401,6 +1401,7 @@ void Memory::clear()
_idMapCount = kIdStart;
_memoryChanged = false;
_linksChanged = false;
_gpsOrigin = GPS();
if(_dbDriver)
{
@@ -3051,7 +3052,7 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
std::string label;
double stamp;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
return pose;
}
@@ -3063,7 +3064,7 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
std::string label;
double stamp;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
return groundTruth;
}
@@ -3076,7 +3077,7 @@ bool Memory::getNodeInfo(int signatureId,
double & stamp,
Transform & groundTruth,
std::vector<float> & velocity,
std::vector<double> & gps,
GPS & gps,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
@@ -3967,10 +3968,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setUserDataRaw(data.userDataRaw());
s->sensorData().setGroundTruth(data.groundTruth());
if(!data.gps().empty())
{
s->sensorData().setGPS(data.gps()[0], data.gps()[1], data.gps()[2], data.gps()[3], data.gps()[4], data.gps()[5]);
}
s->sensorData().setGPS(data.gps());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
@@ -3996,9 +3994,39 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
// prior
if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
if(!isIntermediateNode)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
/*if(data.gps().stamp() > 0.0)
{
UWARN("GPS constraint ignored as global pose is also set.");
}*/
}
else if(data.gps().stamp() > 0.0)
{
// TODO: What kind of covariance should we set to have decent gtsam and g2o results!?
/*if(_gpsOrigin.stamp() <= 0.0)
{
_gpsOrigin = data.gps();
}
cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)*0.00000001;
if(data.gps().error() > 0.0)
{
// only set x, y as we don't know variance for other degrees of freedom.
gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1;
gpsInfMatrix.at<double>(2,2) = 100000;
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
}
else
{
UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
}*/
}
}
return s;

1
corelib/src/OdometryORBSLAM2.cpp
View File

@@ -749,7 +749,6 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
#ifdef RTABMAP_ORB_SLAM2
,
orbslam2_(0),
system_(0),
firstFrame_(true)
#endif
{

38
corelib/src/Optimizer.cpp
View File

@@ -181,7 +181,10 @@ void Optimizer::getConnectedGraph(
uFormat("Input links should be unique between two poses (%d->%d).",
iter->second.from(), iter->second.to()).c_str());
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
if(iter->second.from() != iter->second.to())
{
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
}
}
while((depth == 0 || d < depth) && nextDepth.size())
@@ -199,18 +202,26 @@ void Optimizer::getConnectedGraph(
for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
{
int nextId = iter->second;
if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
if(uContains(posesIn, nextId))
{
nextDepth.insert(nextId);
if(ids.find(nextId) == ids.end())
{
nextDepth.insert(nextId);
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
if(depth == 0 || d < depth-1)
{
linksOut.insert(*kter);
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
if(depth == 0 || d < depth-1)
{
linksOut.insert(*kter);
}
else if(curentDepth.find(nextId) != curentDepth.end() ||
ids.find(nextId) != ids.end())
{
linksOut.insert(*kter);
}
}
else if(curentDepth.find(nextId) != curentDepth.end() ||
ids.find(nextId) != ids.end())
else if(*jter == nextId)
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
linksOut.insert(*kter);
}
}
@@ -221,12 +232,13 @@ void Optimizer::getConnectedGraph(
}
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust)
robust_(robust),
priorsIgnored_(priorsIgnored)
{
}
@@ -235,7 +247,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
slam2d_(Parameters::defaultRegForce3DoF()),
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust())
robust_(Parameters::defaultOptimizerRobust()),
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored())
{
parseParameters(parameters);
}
@@ -247,6 +260,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
}
std::map<int, Transform> Optimizer::optimize(

146
corelib/src/OptimizerG2O.cpp
View File

@@ -235,17 +235,19 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
// detect if there is a global pose prior set, if so remove rootId
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
if(!priorsIgnored())
{
if(iter->second.from() == iter->second.to())
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
rootId = 0;
break;
if(iter->second.from() == iter->second.to())
{
rootId = 0;
break;
}
}
}
UDEBUG("fill poses to g2o...");
std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -292,47 +294,50 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(id1 == id2)
{
if(isSlam2d())
if(!priorsIgnored())
{
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
if(isSlam2d())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
priorEdge->setInformation(information);
edge = priorEdge;
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else
{
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose;
pose = a.rotation();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
else
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d pose;
pose = a.rotation();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
priorEdge->setInformation(information);
edge = priorEdge;
}
priorEdge->setInformation(information);
edge = priorEdge;
}
}
else
@@ -462,7 +467,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
if (!optimizer.addEdge(edge))
if (edge && !optimizer.addEdge(edge))
{
delete edge;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
@@ -767,34 +772,37 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
UDEBUG("added edge %d->%d (in cam frame=%s)",
id1,
id2,
camLink.prettyPrint().c_str());
Eigen::Affine3d a = camLink.toEigen3d();
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
e->setInformation(information);
if (!optimizer.addEdge(e))
if(id1 != id2) // not supporting prior
{
delete e;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
return optimizedPoses;
UASSERT(!iter->second.transform().isNull());
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
UDEBUG("added edge %d->%d (in cam frame=%s)",
id1,
id2,
camLink.prettyPrint().c_str());
Eigen::Affine3d a = camLink.toEigen3d();
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
e->setInformation(information);
if (!optimizer.addEdge(e))
{
delete e;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
return optimizedPoses;
}
}
}
}

217
corelib/src/OptimizerGTSAM.cpp
View File

@@ -99,18 +99,34 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
gtsam::NonlinearFactorGraph graph;
//prior first pose
UASSERT(uContains(poses, rootId));
const Transform & initialPose = poses.at(rootId);
if(isSlam2d())
// detect if there is a global pose prior set, if so remove rootId
if(!priorsIgnored())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() == iter->second.to())
{
rootId = 0;
break;
}
}
}
else
//prior first pose
if(rootId != 0)
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
UASSERT(uContains(poses, rootId));
const Transform & initialPose = poses.at(rootId);
if(isSlam2d())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
}
else
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
}
}
UDEBUG("fill poses to gtsam...");
@@ -134,93 +150,130 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)
{
// not supporting pose prior
continue;
}
UASSERT(!iter->second.transform().isNull());
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
double prior = 1.0;
initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
if(!priorsIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
}
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
double prior = 1.0;
initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
}
}
}

23
corelib/src/Rtabmap.cpp
View File

@@ -761,7 +761,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
std::string l;
double stamp = 0.0;
std::vector<float> v;
std::vector<double> gps;
GPS gps;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
@@ -2651,7 +2651,7 @@ bool Rtabmap::process(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
@@ -2665,10 +2665,7 @@ bool Rtabmap::process(
{
signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures.at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
signatures.at(iter->first).sensorData().setGPS(gps);
}
localGraphSize = (int)poses.size();
if(!lastSignatureLocalizedPose.isNull())
@@ -3346,7 +3343,7 @@ void Rtabmap::get3DMap(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
@@ -3370,10 +3367,7 @@ void Rtabmap::get3DMap(
{
signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures.at(*iter).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
signatures.at(*iter).sensorData().setGPS(gps);
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3426,7 +3420,7 @@ void Rtabmap::getGraph(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
@@ -3455,10 +3449,7 @@ void Rtabmap::getGraph(
{
signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures->at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
signatures->at(iter->first).sensorData().setGPS(gps);
}
}
}