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

Moved some util3d methods to Transform.h, Graph.h and Compression.h

Browse Source 
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
2015-01-23 11:17:42 -05:00
parent fdf7f69783
commit 7a6bf630ca
34 changed files with 1817 additions and 1282 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -27,6 +27,8 @@ SET(SRC_FILES
util3d.cpp
Odometry.cpp
SensorData.cpp
Graph.cpp
Compression.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp

247
corelib/src/Compression.cpp Normal file
View File

@@ -0,0 +1,247 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Compression.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <opencv2/opencv.hpp>
#include <zlib.h>
namespace rtabmap {
// 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();
}
// ".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;
}
} /* namespace rtabmap */

7
corelib/src/DBReader.cpp
View File

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
namespace rtabmap {
@@ -212,9 +213,9 @@ SensorData DBReader::getNextData()
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctLaserScan(laserScanBytes, false);
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();

755
corelib/src/Graph.cpp Normal file
View File

@@ -0,0 +1,755 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <queue>
#include "toro3d/treeoptimizer3.hh"
namespace rtabmap {
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!
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(iter->second.toEigen3f(), 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(iter->second.transform().toEigen3f(), 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 = Transform::fromEigen3f(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 = Transform::fromEigen3f(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(iter->second.toEigen3f(), 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(iter->second.transform().toEigen3f(), 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 = Transform::fromEigen3f(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 = Transform::fromEigen3f(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 = currentT.toEigen3f().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 = checkT.toEigen3f().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 = currentT.toEigen3f().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 = checkT.toEigen3f().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;
}
class Node
{
public:
Node(int id, int fromId, const rtabmap::Transform & pose) :
id_(id),
costSoFar_(0.0f),
distToEnd_(0.0f),
fromId_(fromId),
closed_(false),
pose_(pose)
{}
int id() const {return id_;}
int fromId() const {return fromId_;}
bool isClosed() const {return closed_;}
bool isOpened() const {return !closed_;}
float costSoFar() const {return costSoFar_;} // Dijkstra cost
float distToEnd() const {return distToEnd_;} // Breath-first cost
float totalCost() const {return costSoFar_ + distToEnd_;} // A* cost
rtabmap::Transform pose() const {return pose_;}
float distFrom(const rtabmap::Transform & pose) const
{
return pose_.getDistance(pose);
}
void setClosed(bool closed) {closed_ = closed;}
void setFromId(int fromId) {fromId_ = fromId;}
void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
private:
int id_;
float costSoFar_;
float distToEnd_;
int fromId_;
bool closed_;
rtabmap::Transform pose_;
};
typedef std::pair<int, float> Pair; // first is id, second is cost
struct Order
{
bool operator()(Pair const& a, Pair const& b) const
{
return a.second > b.second;
}
};
std::vector<int> computePath(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, int> & links,
int from,
int to)
{
std::list<int> path;
//A*
int startNode = from;
int endNode = to;
rtabmap::Transform endPose = poses.at(endNode);
std::map<int, Node> nodes;
nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
std::priority_queue<Pair, std::vector<Pair>, Order> pq;
pq.push(Pair(startNode, 0));
while(pq.size())
{
Node & currentNode = nodes.find(pq.top().first)->second;
pq.pop();
currentNode.setClosed(true);
if(currentNode.id() == endNode)
{
while(currentNode.id()!=startNode)
{
path.push_front(currentNode.id());
currentNode = nodes.find(currentNode.fromId())->second;
}
break;
}
// lookup neighbors
for(std::multimap<int, int>::const_iterator iter = links.find(currentNode.id());
iter!=links.end() && iter->first == currentNode.id();
++iter)
{
std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
if(nodeIter == nodes.end())
{
std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
UASSERT(poseIter != poses.end());
Node n(iter->second, currentNode.id(), poseIter->second);
n.setCostSoFar(currentNode.costSoFar() + currentNode.distFrom(poseIter->second));
n.setDistToEnd(n.distFrom(endPose));
nodes.insert(std::make_pair(iter->second, n));
pq.push(Pair(n.id(), n.totalCost()));
}
else if(nodeIter->second.isOpened())
{
float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose());
if(nodeIter->second.costSoFar() > newCostSoFar)
{
UERROR("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
}
}
}
}
return uListToVector(path);
}
} /* namespace rtabmap */

17
corelib/src/Memory.cpp
View File

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "DBDriverSqlite3.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Compression.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
@@ -1658,7 +1659,7 @@ Transform Memory::computeVisualTransform(
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
}
else if(inliersCount < _bowMinInliers)
@@ -1923,7 +1924,7 @@ Transform Memory::computeIcpTransform(
// We are 2D here, make sure the guess has only YAW rotation
float x,y,z,r,p,yaw;
guess.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
guess = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
guess = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
if(r!=0 || p!=0)
{
UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p);
@@ -2040,7 +2041,7 @@ Transform Memory::computeScanMatchingTransform(
const Signature * s = this->getSignature(iter->first);
if(!s->getLaserScanCompressed().empty())
{
*assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getLaserScanCompressed()), iter->second);
*assembledOldClouds += *util3d::cvMat2Cloud(rtabmap::uncompressData(s->getLaserScanCompressed()), iter->second);
}
else
{
@@ -2059,7 +2060,7 @@ Transform Memory::computeScanMatchingTransform(
const Signature * newS = getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
UASSERT(uContains(poses, newId));
newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
newCloud = util3d::cvMat2Cloud(rtabmap::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
//voxelize
if(newCloud->size() && _icp2VoxelSize > 0.0f)
@@ -3302,9 +3303,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
depthOrRightImage = data.rightImage();
}
util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
util3d::CompressionThread ctDepth2d(data.laserScan());
rtabmap::CompressionThread ctImage(data.image(), std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(data.laserScan());
ctImage.start();
ctDepth.start();
ctDepth2d.start();
@@ -3333,7 +3334,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words,
words3D,
data.pose(),
util3d::compressData2(data.laserScan()));
rtabmap::compressData2(data.laserScan()));
}
if(this->isRawDataKept())
{

8
corelib/src/Rtabmap.cpp
View File

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
@@ -646,7 +646,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
util3d::saveTOROGraph(path, poses, constraints);
rtabmap::saveTOROGraph(path, poses, constraints);
}
}
@@ -1264,6 +1264,8 @@ bool Rtabmap::process(const SensorData & data)
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
@@ -2010,7 +2012,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
rtabmap::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
}
}
}

8
corelib/src/Signature.cpp
View File

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/UtiLite.h>
@@ -283,9 +283,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()))
{
util3d::CompressionThread ctImage(_imageCompressed, true);
util3d::CompressionThread ctDepth(_depthCompressed, true);
util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();

84
corelib/src/Transform.cpp
View File

@@ -74,7 +74,7 @@ Transform::Transform(float r11, float r12, float r13, float o14,
Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
{
Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
*this = util3d::transformFromEigen3f(t);
*this = fromEigen3f(t);
}
bool Transform::isNull() const
@@ -133,8 +133,7 @@ void Transform::setIdentity()
Transform Transform::inverse() const
{
Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
return util3d::transformFromEigen4f(m.inverse());
return fromEigen4f(toEigen4f().inverse());
}
Transform Transform::rotation() const
@@ -153,7 +152,7 @@ Transform Transform::translation() const
void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
{
pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
}
void Transform::getTranslation(float & x, float & y, float & z) const
@@ -165,12 +164,22 @@ void Transform::getTranslation(float & x, float & y, float & z) const
float Transform::getNorm() const
{
return std::sqrt(this->getNormSquared());
return uNorm(this->x(), this->y(), this->z());
}
float Transform::getNormSquared() const
{
return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
return uNormSquared(this->x(), this->y(), this->z());
}
float Transform::getDistance(const Transform & t) const
{
return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
}
float Transform::getDistanceSquared(const Transform & t) const
{
return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
}
std::string Transform::prettyPrint() const
@@ -182,9 +191,7 @@ std::string Transform::prettyPrint() const
Transform Transform::operator*(const Transform & t) const
{
Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
return util3d::transformFromEigen4f(m1*m2);
return fromEigen4f(toEigen4f()*t.toEigen4f());
}
Transform & Transform::operator*=(const Transform & t)
@@ -216,5 +223,64 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
return os;
}
Eigen::Matrix4f Transform::toEigen4f() const
{
Eigen::Matrix4f m;
m << data_[0], data_[1], data_[2], data_[3],
data_[4], data_[5], data_[6], data_[7],
data_[8], data_[9], data_[10], data_[11],
0,0,0,1;
return m;
}
Eigen::Matrix4d Transform::toEigen4d() const
{
Eigen::Matrix4d m;
m << data_[0], data_[1], data_[2], data_[3],
data_[4], data_[5], data_[6], data_[7],
data_[8], data_[9], data_[10], data_[11],
0,0,0,1;
return m;
}
Eigen::Affine3f Transform::toEigen3f() const
{
return Eigen::Affine3f(toEigen4f());
}
Eigen::Affine3d Transform::toEigen3d() const
{
return Eigen::Affine3d(toEigen4d());
}
Transform Transform::getIdentity()
{
return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
}
Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3f(const Eigen::Affine3f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
}

831
corelib/src/util3d.cpp
View File

@@ -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,