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

Added version to CMake info. Fixed build WITH_CVSBA.

Browse Source
This commit is contained in:
matlabbe
2016-01-04 16:33:34 -05:00
parent a403a5b7d8
commit bf2bd3fe50
5 changed files with 9 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/OptimizerCVSBA.cpp
View File

@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerCVSBA.h>
@@ -142,7 +137,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
T.resize(oi);
distCoeffs.resize(oi);
std::map<int, pcl::PointXYZ> points3DMap;
std::map<int, cv::Point3f> points3DMap;
std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
@@ -175,8 +170,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
Transform pose = frames.at(sFrom.id());
for(unsigned int i=0; i<inliers.size(); ++i)
{
pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
std::map<int, pcl::PointXYZ>::iterator jter = points3DMap.find(inliers[i]);
cv::Point3f p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
std::map<int, cv::Point3f>::iterator jter = points3DMap.find(inliers[i]);
if(jter == points3DMap.end())
{
points3DMap.insert(std::make_pair(inliers[i], p));
@@ -215,10 +210,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int i=0;
for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
{
pcl::PointXYZ & p = points3DMap.at(*iter);
points[i].x = p.x;
points[i].y = p.y;
points[i].z = p.z;
points[i] = points3DMap.at(*iter);
std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
while(jter->first == *iter && jter != wordReferences.end())

5
corelib/src/OptimizerG2O.cpp
View File

@@ -30,11 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerG2O.h>

5
corelib/src/OptimizerGTSAM.cpp
View File

@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerGTSAM.h>

13
corelib/src/OptimizerTORO.cpp
View File

@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerTORO.h>
@@ -329,7 +324,7 @@ bool OptimizerTORO::saveGraph(
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);
iter->second.getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
iter->first,
x,
@@ -344,7 +339,7 @@ bool OptimizerTORO::saveGraph(
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);
iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
iter->first,
iter->second.to(),
@@ -415,7 +410,7 @@ bool OptimizerTORO::loadGraph(
float roll = uStr2Float(strList[5]);
float pitch = uStr2Float(strList[6]);
float yaw = uStr2Float(strList[7]);
Transform pose = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
Transform pose(x, y, z, roll, pitch, yaw);
if(poses.find(id) == poses.end())
{
poses.insert(std::make_pair(id, pose));
@@ -447,7 +442,7 @@ bool OptimizerTORO::loadGraph(
UASSERT_MSG(infX > 0 && infY > 0 && infZ > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
float transVariance = 1.0f/(infX<=infY && infX<=infZ?infX:infY<=infW?infY:infZ); // maximum variance
UINFO("id=%d rotV=%f transV=%f", idFrom, rotVariance, transVariance);
Transform transform = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
Transform transform(x, y, z, roll, pitch, yaw);
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{
//Link type is unknown