i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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
CMakeLists.txt
+1
View File
@@ -409,6 +409,7 @@ INCLUDE(CPack)
MESSAGE(STATUS "--------------------------------------------") MESSAGE(STATUS "--------------------------------------------")
MESSAGE(STATUS "Info :") MESSAGE(STATUS "Info :")
MESSAGE(STATUS " Version : ${RTABMAP_VERSION}")
MESSAGE(STATUS " CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}") MESSAGE(STATUS " CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}")
MESSAGE(STATUS " CMAKE_BUILD_TYPE = ${CMAKE_BUILD_TYPE}") MESSAGE(STATUS " CMAKE_BUILD_TYPE = ${CMAKE_BUILD_TYPE}")
IF(NOT WIN32) IF(NOT WIN32)
corelib/src/OptimizerCVSBA.cpp
+4 -12
View File
@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.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 <set>
#include <rtabmap/core/OptimizerCVSBA.h> #include <rtabmap/core/OptimizerCVSBA.h>
@@ -142,7 +137,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
T.resize(oi); T.resize(oi);
distCoeffs.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> 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) 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()); Transform pose = frames.at(sFrom.id());
for(unsigned int i=0; i<inliers.size(); ++i) for(unsigned int i=0; i<inliers.size(); ++i)
{ {
pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose); cv::Point3f p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
std::map<int, pcl::PointXYZ>::iterator jter = points3DMap.find(inliers[i]); std::map<int, cv::Point3f>::iterator jter = points3DMap.find(inliers[i]);
if(jter == points3DMap.end()) if(jter == points3DMap.end())
{ {
points3DMap.insert(std::make_pair(inliers[i], p)); points3DMap.insert(std::make_pair(inliers[i], p));
@@ -215,10 +210,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int i=0; int i=0;
for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter) for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
{ {
pcl::PointXYZ & p = points3DMap.at(*iter); points[i] = points3DMap.at(*iter);
points[i].x = p.x;
points[i].y = p.y;
points[i].z = p.z;
std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter); std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
while(jter->first == *iter && jter != wordReferences.end()) while(jter->first == *iter && jter != wordReferences.end())
corelib/src/OptimizerG2O.cpp
-5
View File
@@ -30,11 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.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 <set>
#include <rtabmap/core/OptimizerG2O.h> #include <rtabmap/core/OptimizerG2O.h>
corelib/src/OptimizerGTSAM.cpp
-5
View File
@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.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 <set>
#include <rtabmap/core/OptimizerGTSAM.h> #include <rtabmap/core/OptimizerGTSAM.h>
corelib/src/OptimizerTORO.cpp
+4 -9
View File
@@ -31,11 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.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 <set>
#include <rtabmap/core/OptimizerTORO.h> #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) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
float x,y,z, yaw,pitch,roll; 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", fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
iter->first, iter->first,
x, x,
@@ -344,7 +339,7 @@ bool OptimizerTORO::saveGraph(
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {
float x,y,z, yaw,pitch,roll; 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", 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->first,
iter->second.to(), iter->second.to(),
@@ -415,7 +410,7 @@ bool OptimizerTORO::loadGraph(
float roll = uStr2Float(strList[5]); float roll = uStr2Float(strList[5]);
float pitch = uStr2Float(strList[6]); float pitch = uStr2Float(strList[6]);
float yaw = uStr2Float(strList[7]); 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()) if(poses.find(id) == poses.end())
{ {
poses.insert(std::make_pair(id, pose)); 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()); 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 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); 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()) if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{ {
//Link type is unknown //Link type is unknown