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

DbSqlite3: fixed Feature.depth_[x,y,z] not saving correctly NaN values (then NaNs were wrongly converted to 0 on reload). Gui: Added stereo exposure compensation option in Source panel.

Browse Source
This commit is contained in:
matlabbe
2018-02-11 16:07:26 -05:00
parent 1c8c233ebf
commit 1595405871
9 changed files with 114 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2644,6 +2644,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
@@ -2676,9 +2678,36 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
kpt.octave = sqlite3_column_int(ppStmt, index++);
}
depth.x = sqlite3_column_double(ppStmt, index++);
depth.y = sqlite3_column_double(ppStmt, index++);
depth.z = sqlite3_column_double(ppStmt, index++);
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.x = nanFloat;
++index;
}
else
{
depth.x = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.y = nanFloat;
++index;
}
else
{
depth.y = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.z = nanFloat;
++index;
}
else
{
depth.z = sqlite3_column_double(ppStmt, index++);
}
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
@@ -5306,12 +5335,39 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_int(ppStmt, index++, kp.octave);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_double(ppStmt, index++, pt.x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, pt.y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, pt.z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uIsFinite(pt.x))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uIsFinite(pt.y))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uIsFinite(pt.z))
{
rc = sqlite3_bind_double(ppStmt, index++, pt.z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//descriptor
if(uStrNumCmp(_version, "0.11.2") >= 0)

6
corelib/src/RegistrationVis.cpp
View File

@@ -1602,7 +1602,6 @@ Transform RegistrationVis::computeTransformationImpl(
{
transforms[0] = optimizedPoses.rbegin()->second;
}
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
@@ -1618,6 +1617,11 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
}
else
{
transforms[0].setNull();
}
transforms[1].setNull();
}
info.inliersIDs = allInliers;

4
corelib/src/util3d_motion_estimation.cpp
View File

@@ -167,10 +167,11 @@ Transform estimateMotion3DTo2D(
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
@@ -188,6 +189,7 @@ Transform estimateMotion3DTo2D(
{
err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
}
UASSERT(uIsFinite(err));
*covariance *= std::sqrt(err/float(inliers.size()));
}
}

5
corelib/src/util3d_registration.cpp
View File

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/sample_consensus/ransac.h>
#include <pcl/common/common.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
namespace rtabmap
{
@@ -202,7 +203,9 @@ Transform transformFromXYZCorrespondences(
}
if(covariance)
{
*covariance *= model->computeVariance();
double variance = model->computeVariance();
UASSERT(uIsFinite(variance));
*covariance *= variance;
}
// get best transformation