mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 01:50:24 +08:00
Moved some util3d methods to Transform.h, Graph.h and Compression.h
Added computePath() method implementating A star on graph GUI: SetWindowModified() and user should explicitly save the GUI config to keep them Calibration: added mirror checkbox, added device id argument
This commit is contained in:
Mathieu Labbe
@@ -47,13 +47,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <cmath>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <zlib.h>
|
||||
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/UTimer.h"
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "toro3d/treeoptimizer3.hh"
|
||||
|
||||
#include <pcl/filters/random_sample.h>
|
||||
|
||||
@@ -63,54 +60,6 @@ namespace rtabmap
|
||||
namespace util3d
|
||||
{
|
||||
|
||||
// format : ".png" ".jpg" "" (empty is general)
|
||||
CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
|
||||
uncompressedData_(mat),
|
||||
format_(format),
|
||||
image_(!format.empty()),
|
||||
compressMode_(true)
|
||||
{
|
||||
UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
|
||||
}
|
||||
// assume image
|
||||
CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
|
||||
compressedData_(bytes),
|
||||
image_(isImage),
|
||||
compressMode_(false)
|
||||
{}
|
||||
void CompressionThread::mainLoop()
|
||||
{
|
||||
if(compressMode_)
|
||||
{
|
||||
if(!uncompressedData_.empty())
|
||||
{
|
||||
if(image_)
|
||||
{
|
||||
compressedData_ = compressImage2(uncompressedData_, format_);
|
||||
}
|
||||
else
|
||||
{
|
||||
compressedData_ = compressData2(uncompressedData_);
|
||||
}
|
||||
}
|
||||
}
|
||||
else // uncompress
|
||||
{
|
||||
if(!compressedData_.empty())
|
||||
{
|
||||
if(image_)
|
||||
{
|
||||
uncompressedData_ = uncompressImage(compressedData_);
|
||||
}
|
||||
else
|
||||
{
|
||||
uncompressedData_ = uncompressData(compressedData_);
|
||||
}
|
||||
}
|
||||
}
|
||||
this->kill();
|
||||
}
|
||||
|
||||
cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
|
||||
{
|
||||
cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
|
||||
@@ -345,7 +294,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
|
||||
|
||||
if(!transform.isNull() && !transform.isIdentity())
|
||||
{
|
||||
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
|
||||
pt = pcl::transformPoint(pt, transform.toEigen3f());
|
||||
}
|
||||
keypoints3d->at(i) = pt;
|
||||
}
|
||||
@@ -377,7 +326,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
|
||||
|
||||
if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
|
||||
{
|
||||
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
|
||||
pt = pcl::transformPoint(pt, transform.toEigen3f());
|
||||
}
|
||||
keypoints3d->at(i) = pt;
|
||||
}
|
||||
@@ -444,7 +393,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
|
||||
pt = tmpPt;
|
||||
if(!transform.isNull() && !transform.isIdentity())
|
||||
{
|
||||
pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
|
||||
pt = pcl::transformPoint(pt, transform.toEigen3f());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1095,168 +1044,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
|
||||
return output;
|
||||
}
|
||||
|
||||
// ".png" or ".jpg"
|
||||
std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
|
||||
{
|
||||
std::vector<unsigned char> bytes;
|
||||
if(!image.empty())
|
||||
{
|
||||
cv::imencode(format, image, bytes);
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
// ".png" or ".jpg"
|
||||
cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
|
||||
{
|
||||
std::vector<unsigned char> bytes = compressImage(image, format);
|
||||
if(bytes.size())
|
||||
{
|
||||
return cv::Mat(1, bytes.size(), CV_8UC1, bytes.data()).clone();
|
||||
}
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const cv::Mat & bytes)
|
||||
{
|
||||
cv::Mat image;
|
||||
if(!bytes.empty())
|
||||
{
|
||||
#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
|
||||
image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
|
||||
#else
|
||||
image = cv::imdecode(bytes, -1);
|
||||
#endif
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
cv::Mat image;
|
||||
if(bytes.size())
|
||||
{
|
||||
#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
|
||||
image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
|
||||
#else
|
||||
image = cv::imdecode(bytes, -1);
|
||||
#endif
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
std::vector<unsigned char> compressData(const cv::Mat & data)
|
||||
{
|
||||
std::vector<unsigned char> bytes;
|
||||
if(!data.empty())
|
||||
{
|
||||
uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
|
||||
uLong destLen = compressBound(sourceLen);
|
||||
bytes.resize(destLen);
|
||||
int errCode = compress(
|
||||
(Bytef *)bytes.data(),
|
||||
&destLen,
|
||||
(const Bytef *)data.data,
|
||||
sourceLen);
|
||||
|
||||
bytes.resize(destLen+3*sizeof(int));
|
||||
*((int*)&bytes[destLen]) = data.rows;
|
||||
*((int*)&bytes[destLen+sizeof(int)]) = data.cols;
|
||||
*((int*)&bytes[destLen+2*sizeof(int)]) = data.type();
|
||||
|
||||
if(errCode == Z_MEM_ERROR)
|
||||
{
|
||||
UERROR("Z_MEM_ERROR : Insufficient memory.");
|
||||
}
|
||||
else if(errCode == Z_BUF_ERROR)
|
||||
{
|
||||
UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
|
||||
}
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
cv::Mat compressData2(const cv::Mat & data)
|
||||
{
|
||||
cv::Mat bytes;
|
||||
if(!data.empty())
|
||||
{
|
||||
uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
|
||||
uLong destLen = compressBound(sourceLen);
|
||||
bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1);
|
||||
int errCode = compress(
|
||||
(Bytef *)bytes.data,
|
||||
&destLen,
|
||||
(const Bytef *)data.data,
|
||||
sourceLen);
|
||||
bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1));
|
||||
*((int*)&bytes.data[destLen]) = data.rows;
|
||||
*((int*)&bytes.data[destLen+sizeof(int)]) = data.cols;
|
||||
*((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type();
|
||||
|
||||
if(errCode == Z_MEM_ERROR)
|
||||
{
|
||||
UERROR("Z_MEM_ERROR : Insufficient memory.");
|
||||
}
|
||||
else if(errCode == Z_BUF_ERROR)
|
||||
{
|
||||
UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
|
||||
}
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
cv::Mat uncompressData(const cv::Mat & bytes)
|
||||
{
|
||||
UASSERT(bytes.empty() || bytes.type() == CV_8UC1);
|
||||
return uncompressData(bytes.data, bytes.cols*bytes.rows);
|
||||
}
|
||||
|
||||
cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressData(bytes.data(), bytes.size());
|
||||
}
|
||||
|
||||
cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
|
||||
{
|
||||
cv::Mat data;
|
||||
if(bytes && size>=3*sizeof(int))
|
||||
{
|
||||
//last 3 int elements are matrix size and type
|
||||
int height = *((int*)&bytes[size-3*sizeof(int)]);
|
||||
int width = *((int*)&bytes[size-2*sizeof(int)]);
|
||||
int type = *((int*)&bytes[size-1*sizeof(int)]);
|
||||
|
||||
// If the size is higher, it may be a wrong data format.
|
||||
UASSERT_MSG(height>=0 && height<10000 &&
|
||||
width>=0 && width<10000,
|
||||
uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
|
||||
|
||||
data = cv::Mat(height, width, type);
|
||||
uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
|
||||
|
||||
int errCode = uncompress(
|
||||
(Bytef*)data.data,
|
||||
&totalUncompressed,
|
||||
(const Bytef*)bytes,
|
||||
uLong(size));
|
||||
|
||||
if(errCode == Z_MEM_ERROR)
|
||||
{
|
||||
UERROR("Z_MEM_ERROR : Insufficient memory.");
|
||||
}
|
||||
else if(errCode == Z_BUF_ERROR)
|
||||
{
|
||||
UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
|
||||
}
|
||||
else if(errCode == Z_DATA_ERROR)
|
||||
{
|
||||
UERROR("Z_DATA_ERROR : The compressed data (referenced by source) was corrupted.");
|
||||
}
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
|
||||
const std::multimap<int, pcl::PointXYZ> & words2,
|
||||
pcl::PointCloud<pcl::PointXYZ> & cloud1,
|
||||
@@ -1643,7 +1430,7 @@ Transform transformFromXYZCorrespondences(
|
||||
bestTransformation.row (2) = model_coefficients.segment<4>(8);
|
||||
bestTransformation.row (3) = model_coefficients.segment<4>(12);
|
||||
|
||||
transform = util3d::transformFromEigen4f(bestTransformation);
|
||||
transform = Transform::fromEigen4f(bestTransformation);
|
||||
UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
|
||||
|
||||
return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
|
||||
@@ -1747,7 +1534,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
|
||||
*hasConvergedOut = hasConverged;
|
||||
}
|
||||
|
||||
return transformFromEigen4f(icp.getFinalTransformation());
|
||||
return Transform::fromEigen4f(icp.getFinalTransformation());
|
||||
}
|
||||
|
||||
// return transform from source to target (All points/normals must be finite!!!)
|
||||
@@ -1837,7 +1624,7 @@ Transform icpPointToPlane(
|
||||
*hasConvergedOut = hasConverged;
|
||||
}
|
||||
|
||||
return transformFromEigen4f(icp.getFinalTransformation());
|
||||
return Transform::fromEigen4f(icp.getFinalTransformation());
|
||||
}
|
||||
|
||||
// return transform from source to target (All points must be finite!!!)
|
||||
@@ -1926,7 +1713,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
|
||||
*hasConvergedOut = hasConverged;
|
||||
}
|
||||
|
||||
return transformFromEigen4f(icp.getFinalTransformation());
|
||||
return Transform::fromEigen4f(icp.getFinalTransformation());
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
|
||||
@@ -2054,7 +1841,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
|
||||
UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
|
||||
UASSERT(matrix.rows == 1);
|
||||
|
||||
Eigen::Affine3f t = transformToEigen3f(tranform);
|
||||
Eigen::Affine3f t = tranform.toEigen3f();
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
cloud->resize(matrix.cols);
|
||||
if(matrix.channels() == 2)
|
||||
@@ -2219,608 +2006,6 @@ pcl::PolygonMesh::Ptr createMesh(
|
||||
return mesh;
|
||||
}
|
||||
|
||||
std::multimap<int, Link>::iterator findLink(
|
||||
std::multimap<int, Link> & links,
|
||||
int from,
|
||||
int to)
|
||||
{
|
||||
std::multimap<int, Link>::iterator iter = links.find(from);
|
||||
while(iter != links.end() && iter->first == from)
|
||||
{
|
||||
if(iter->second.to() == to)
|
||||
{
|
||||
return iter;
|
||||
}
|
||||
++iter;
|
||||
}
|
||||
|
||||
// let's try to -> from
|
||||
iter = links.find(to);
|
||||
while(iter != links.end() && iter->first == to)
|
||||
{
|
||||
if(iter->second.to() == from)
|
||||
{
|
||||
return iter;
|
||||
}
|
||||
++iter;
|
||||
}
|
||||
return links.end();
|
||||
}
|
||||
|
||||
|
||||
// <int, depth> margin=0 means infinite margin
|
||||
std::map<int, int> generateDepthGraph(
|
||||
const std::multimap<int, Link> & links,
|
||||
int fromId,
|
||||
int depth)
|
||||
{
|
||||
UASSERT(depth >= 0);
|
||||
//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
|
||||
std::map<int, int> ids;
|
||||
if(fromId<=0)
|
||||
{
|
||||
return ids;
|
||||
}
|
||||
|
||||
std::list<int> curentDepthList;
|
||||
std::set<int> nextDepth;
|
||||
nextDepth.insert(fromId);
|
||||
int d = 0;
|
||||
while((depth == 0 || d < depth) && nextDepth.size())
|
||||
{
|
||||
curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
|
||||
nextDepth.clear();
|
||||
|
||||
for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
|
||||
{
|
||||
if(ids.find(*jter) == ids.end())
|
||||
{
|
||||
std::set<int> marginIds;
|
||||
|
||||
ids.insert(std::pair<int, int>(*jter, d));
|
||||
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(iter->second.from() == *jter)
|
||||
{
|
||||
marginIds.insert(iter->second.to());
|
||||
}
|
||||
else if(iter->second.to() == *jter)
|
||||
{
|
||||
marginIds.insert(iter->second.from());
|
||||
}
|
||||
}
|
||||
|
||||
// Margin links
|
||||
for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
|
||||
{
|
||||
if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
|
||||
{
|
||||
nextDepth.insert(*iter);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
++d;
|
||||
}
|
||||
return ids;
|
||||
}
|
||||
|
||||
void optimizeTOROGraph(
|
||||
const std::map<int, int> & depthGraph,
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, Link> & links,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
int toroIterations,
|
||||
bool toroInitialGuess,
|
||||
bool ignoreCovariance,
|
||||
std::list<std::map<int, Transform> > * intermediateGraphes)
|
||||
{
|
||||
optimizedPoses.clear();
|
||||
if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
|
||||
{
|
||||
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
|
||||
int m = 0;
|
||||
int toroId = 1;
|
||||
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
|
||||
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
|
||||
std::map<int, int> idsTmp = depthGraph;
|
||||
while(idsTmp.size())
|
||||
{
|
||||
for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
|
||||
{
|
||||
if(m == iter->second)
|
||||
{
|
||||
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
|
||||
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
|
||||
++toroId;
|
||||
idsTmp.erase(iter++);
|
||||
}
|
||||
else
|
||||
{
|
||||
++iter;
|
||||
}
|
||||
}
|
||||
++m;
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> posesToro;
|
||||
std::multimap<int, rtabmap::Link> edgeConstraintsToro;
|
||||
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
if(uContains(depthGraph, iter->first))
|
||||
{
|
||||
UASSERT(!iter->second.isNull());
|
||||
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
|
||||
}
|
||||
}
|
||||
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
|
||||
iter!=links.end();
|
||||
++iter)
|
||||
{
|
||||
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
|
||||
{
|
||||
UASSERT(!iter->second.transform().isNull());
|
||||
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
|
||||
}
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> optimizedPosesToro;
|
||||
|
||||
if(posesToro.size() && edgeConstraintsToro.size())
|
||||
{
|
||||
std::list<std::map<int, rtabmap::Transform> > graphesToro;
|
||||
|
||||
// Optimize!
|
||||
rtabmap::util3d::optimizeTOROGraph(
|
||||
posesToro,
|
||||
edgeConstraintsToro,
|
||||
optimizedPosesToro,
|
||||
toroIterations,
|
||||
toroInitialGuess,
|
||||
ignoreCovariance,
|
||||
&graphesToro);
|
||||
|
||||
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
|
||||
{
|
||||
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
|
||||
}
|
||||
|
||||
if(intermediateGraphes)
|
||||
{
|
||||
for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
|
||||
{
|
||||
std::map<int, rtabmap::Transform> tmp;
|
||||
for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
|
||||
{
|
||||
tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
|
||||
}
|
||||
intermediateGraphes->push_back(tmp);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("No TORO poses and constraints!?");
|
||||
}
|
||||
}
|
||||
else if(links.size() == 0 && poses.size() == 1)
|
||||
{
|
||||
optimizedPoses = poses;
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
|
||||
(int)depthGraph.size(), (int)poses.size(), (int)links.size());
|
||||
}
|
||||
}
|
||||
|
||||
//On success, optimizedPoses is cleared and new poses are inserted in
|
||||
void optimizeTOROGraph(
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, Link> & edgeConstraints,
|
||||
std::map<int, Transform> & optimizedPoses,
|
||||
int toroIterations,
|
||||
bool toroInitialGuess,
|
||||
bool ignoreCovariance,
|
||||
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
|
||||
{
|
||||
UASSERT(toroIterations>0);
|
||||
optimizedPoses.clear();
|
||||
if(edgeConstraints.size()>=1 && poses.size()>=2)
|
||||
{
|
||||
// Apply TORO optimization
|
||||
AISNavigation::TreeOptimizer3 pg;
|
||||
pg.verboseLevel = 0;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
UASSERT(!iter->second.isNull());
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll,pitch,yaw);
|
||||
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
|
||||
if (v)
|
||||
{
|
||||
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("cannot insert vertex %d!?", iter->first);
|
||||
}
|
||||
}
|
||||
|
||||
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
{
|
||||
int id1 = iter->first;
|
||||
int id2 = iter->second.to();
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
UASSERT(!iter->second.transform().isNull());
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
|
||||
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
|
||||
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
|
||||
if(!ignoreCovariance && iter->second.variance()>0)
|
||||
{
|
||||
inf[0][0] = 1.0f/iter->second.variance(); // x
|
||||
inf[1][1] = 1.0f/iter->second.variance(); // y
|
||||
inf[2][2] = 1.0f/iter->second.variance(); // z
|
||||
inf[3][3] = 1.0f/iter->second.variance(); // roll
|
||||
inf[4][4] = 1.0f/iter->second.variance(); // pitch
|
||||
inf[5][5] = 1.0f/iter->second.variance(); // yaw
|
||||
}
|
||||
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
|
||||
AISNavigation::TreePoseGraph3::Transformation t(p);
|
||||
if (!pg.addEdge(v1, v2, t, inf))
|
||||
{
|
||||
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
|
||||
return;
|
||||
}
|
||||
}
|
||||
pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
|
||||
|
||||
UDEBUG("Initial guess...");
|
||||
if(toroInitialGuess)
|
||||
{
|
||||
pg.initializeOnTree(); // optional
|
||||
}
|
||||
|
||||
pg.initializeTreeParameters();
|
||||
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
|
||||
"TORO is not able to find the root of the graph!)");
|
||||
pg.initializeOptimization();
|
||||
|
||||
UDEBUG("TORO iterate begin (iterations=%d)", toroIterations);
|
||||
for (int i=0; i<toroIterations; i++)
|
||||
{
|
||||
if(intermediateGraphes && (toroInitialGuess || i>0))
|
||||
{
|
||||
std::map<int, Transform> tmpPoses;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
|
||||
v->pose=v->transformation.toPoseType();
|
||||
Transform newPose = transformFromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
|
||||
|
||||
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
|
||||
}
|
||||
intermediateGraphes->push_back(tmpPoses);
|
||||
}
|
||||
|
||||
pg.iterate();
|
||||
}
|
||||
UDEBUG("TORO iterate end");
|
||||
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
|
||||
v->pose=v->transformation.toPoseType();
|
||||
Transform newPose = transformFromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
|
||||
|
||||
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
|
||||
}
|
||||
|
||||
//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
|
||||
//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
|
||||
//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
|
||||
//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
|
||||
//mapCorrection = transformFromEigen4f(result);
|
||||
}
|
||||
else if(edgeConstraints.size() == 0 && poses.size() == 1)
|
||||
{
|
||||
optimizedPoses = poses;
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("This method should be called at least with 1 pose!");
|
||||
}
|
||||
}
|
||||
|
||||
bool saveTOROGraph(
|
||||
const std::string & fileName,
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::multimap<int, Link> & edgeConstraints)
|
||||
{
|
||||
FILE * file = 0;
|
||||
|
||||
#ifdef _MSC_VER
|
||||
fopen_s(&file, fileName.c_str(), "w");
|
||||
#else
|
||||
file = fopen(fileName.c_str(), "w");
|
||||
#endif
|
||||
|
||||
if(file)
|
||||
{
|
||||
// VERTEX3 id x y z phi theta psi
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
float x,y,z, yaw,pitch,roll;
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll, pitch, yaw);
|
||||
fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
|
||||
iter->first,
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
roll,
|
||||
pitch,
|
||||
yaw);
|
||||
}
|
||||
|
||||
//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
|
||||
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
|
||||
{
|
||||
float x,y,z, yaw,pitch,roll;
|
||||
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
|
||||
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
|
||||
iter->first,
|
||||
iter->second.to(),
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
1.0f/iter->second.variance(),
|
||||
1.0f/iter->second.variance(),
|
||||
1.0f/iter->second.variance(),
|
||||
1.0f/iter->second.variance(),
|
||||
1.0f/iter->second.variance(),
|
||||
1.0f/iter->second.variance());
|
||||
}
|
||||
UINFO("Graph saved to %s", fileName.c_str());
|
||||
fclose(file);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Cannot save to file %s", fileName.c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool loadTOROGraph(const std::string & fileName,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, std::pair<int, Transform> > & edgeConstraints)
|
||||
{
|
||||
FILE * file = 0;
|
||||
#ifdef _MSC_VER
|
||||
fopen_s(&file, fileName.c_str(), "r");
|
||||
#else
|
||||
file = fopen(fileName.c_str(), "r");
|
||||
#endif
|
||||
|
||||
if(file)
|
||||
{
|
||||
char line[200];
|
||||
while ( fgets (line , 200 , file) != NULL )
|
||||
{
|
||||
std::vector<std::string> strList = uListToVector(uSplit(line, ' '));
|
||||
if(strList.size() == 8)
|
||||
{
|
||||
//VERTEX3
|
||||
int id = atoi(strList[1].c_str());
|
||||
float x = atof(strList[2].c_str());
|
||||
float y = atof(strList[3].c_str());
|
||||
float z = atof(strList[4].c_str());
|
||||
float roll = atof(strList[5].c_str());
|
||||
float pitch = atof(strList[6].c_str());
|
||||
float yaw = atof(strList[7].c_str());
|
||||
Transform pose = transformFromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
|
||||
std::map<int, Transform>::iterator iter = poses.find(id);
|
||||
if(iter != poses.end())
|
||||
{
|
||||
iter->second = pose;
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("");
|
||||
}
|
||||
}
|
||||
else if(strList.size() == 30)
|
||||
{
|
||||
//EDGE3
|
||||
int idFrom = atoi(strList[1].c_str());
|
||||
int idTo = atoi(strList[2].c_str());
|
||||
float x = atof(strList[3].c_str());
|
||||
float y = atof(strList[4].c_str());
|
||||
float z = atof(strList[5].c_str());
|
||||
float roll = atof(strList[6].c_str());
|
||||
float pitch = atof(strList[7].c_str());
|
||||
float yaw = atof(strList[8].c_str());
|
||||
Transform transform = transformFromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
|
||||
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
|
||||
{
|
||||
std::pair<int, Transform> edge(idTo, transform);
|
||||
edgeConstraints.insert(std::pair<int, std::pair<int, Transform> >(idFrom, edge));
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Error parsing map file %s", fileName.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
UINFO("Graph loaded from %s", fileName.c_str());
|
||||
fclose(file);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Cannot open file %s", fileName.c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
|
||||
{
|
||||
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
cloud->resize(poses.size());
|
||||
int i=0;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
|
||||
}
|
||||
|
||||
// radius filtering
|
||||
std::vector<int> names = uKeys(poses);
|
||||
std::vector<Transform> transforms = uValues(poses);
|
||||
|
||||
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
|
||||
tree->setInputCloud(cloud);
|
||||
std::set<int> indicesChecked;
|
||||
std::set<int> indicesKept;
|
||||
|
||||
for(unsigned int i=0; i<cloud->size(); ++i)
|
||||
{
|
||||
// ignore scans
|
||||
if(indicesChecked.find(i) == indicesChecked.end())
|
||||
{
|
||||
std::vector<int> kIndices;
|
||||
std::vector<float> kDistances;
|
||||
tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
|
||||
|
||||
std::set<int> cloudIndices;
|
||||
const Transform & currentT = transforms.at(i);
|
||||
Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
|
||||
for(unsigned int j=0; j<kIndices.size(); ++j)
|
||||
{
|
||||
if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
|
||||
{
|
||||
const Transform & checkT = transforms.at(kIndices[j]);
|
||||
// same orientation?
|
||||
Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
|
||||
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
|
||||
if(a <= angle)
|
||||
{
|
||||
cloudIndices.insert(kIndices[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(keepLatest)
|
||||
{
|
||||
bool lastAdded = false;
|
||||
for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
|
||||
{
|
||||
if(!lastAdded)
|
||||
{
|
||||
indicesKept.insert(*iter);
|
||||
lastAdded = true;
|
||||
}
|
||||
indicesChecked.insert(*iter);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bool firstAdded = false;
|
||||
for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
|
||||
{
|
||||
if(!firstAdded)
|
||||
{
|
||||
indicesKept.insert(*iter);
|
||||
firstAdded = true;
|
||||
}
|
||||
indicesChecked.insert(*iter);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//pcl::IndicesPtr indicesOut(new std::vector<int>);
|
||||
//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
|
||||
UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
|
||||
//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
|
||||
//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
|
||||
|
||||
std::map<int, Transform> keptPoses;
|
||||
for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
|
||||
{
|
||||
keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
|
||||
}
|
||||
|
||||
return keptPoses;
|
||||
}
|
||||
else
|
||||
{
|
||||
return poses;
|
||||
}
|
||||
}
|
||||
|
||||
std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
|
||||
{
|
||||
std::multimap<int, int> clusters;
|
||||
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
cloud->resize(poses.size());
|
||||
int i=0;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
|
||||
}
|
||||
|
||||
// radius clustering (nearest neighbors)
|
||||
std::vector<int> ids = uKeys(poses);
|
||||
std::vector<Transform> transforms = uValues(poses);
|
||||
|
||||
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
|
||||
tree->setInputCloud(cloud);
|
||||
|
||||
for(unsigned int i=0; i<cloud->size(); ++i)
|
||||
{
|
||||
std::vector<int> kIndices;
|
||||
std::vector<float> kDistances;
|
||||
tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
|
||||
|
||||
std::set<int> cloudIndices;
|
||||
const Transform & currentT = transforms.at(i);
|
||||
Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
|
||||
for(unsigned int j=0; j<kIndices.size(); ++j)
|
||||
{
|
||||
if((int)i != kIndices[j])
|
||||
{
|
||||
const Transform & checkT = transforms.at(kIndices[j]);
|
||||
// same orientation?
|
||||
Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
|
||||
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
|
||||
if(a <= angle)
|
||||
{
|
||||
clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return clusters;
|
||||
}
|
||||
|
||||
bool occupancy2DFromCloud3D(
|
||||
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
|
||||
cv::Mat & ground,
|
||||
|
||||
Reference in New Issue
Block a user