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

Fixed odometryBOW to correctly set initial pose

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1932 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 01:58:39 +00:00
parent b56e03b463
commit d950c88373
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Odometry.cpp
View File

@@ -393,6 +393,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
int count = 0; int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3()); std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter) for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{ {
// Only add unique words // Only add unique words
@@ -401,7 +402,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second; const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt)) if(pcl::isFinite(pt))
{ {
localMap_.insert(std::make_pair(*iter, pt)); pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
} }
else else
{ {