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 multi-camera feature

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:46:48 -04:00
parent e6923daf1c
commit c6d0d47b1c
51 changed files with 2833 additions and 2297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
corelib/src/CameraModel.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
@@ -39,13 +40,21 @@ CameraModel::CameraModel() :
}
CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P) :
CameraModel::CameraModel(
const std::string & cameraName,
const cv::Size & imageSize,
const cv::Mat & K,
const cv::Mat & D,
const cv::Mat & R,
const cv::Mat & P,
const Transform & localTransform) :
name_(cameraName),
imageSize_(imageSize),
K_(K),
D_(D),
R_(R),
P_(P)
P_(P),
localTransform_(localTransform)
{
UASSERT(!name_.empty());
UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
@@ -59,6 +68,35 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
CameraModel::CameraModel(
double fx,
double fy,
double cx,
double cy,
const Transform & localTransform,
double Tx) :
K_(cv::Mat::eye(3, 3, CV_64FC1)),
D_(cv::Mat::zeros(1, 5, CV_64FC1)),
R_(cv::Mat::eye(3, 3, CV_64FC1)),
P_(cv::Mat::eye(3, 4, CV_64FC1)),
localTransform_(localTransform)
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
K_.at<double>(0,0) = fx;
K_.at<double>(1,1) = fy;
K_.at<double>(0,2) = cx;
K_.at<double>(1,2) = cy;
}
bool CameraModel::load(const std::string & filePath)
{
K_ = cv::Mat();
@@ -172,6 +210,22 @@ bool CameraModel::save(const std::string & filePath)
return false;
}
void CameraModel::scale(double scale)
{
UASSERT(scale > 0.0);
// has only effect on K and P
imageSize_.width *= scale;
imageSize_.height *= scale;
K_.at<double>(0,0) *= scale;
K_.at<double>(1,1) *= scale;
K_.at<double>(0,2) *= scale;
K_.at<double>(1,2) *= scale;
P_.at<double>(0,0) *= scale;
P_.at<double>(1,1) *= scale;
P_.at<double>(0,2) *= scale;
P_.at<double>(1,2) *= scale;
}
cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
{
if(!mapX_.empty() && !mapY_.empty())
@@ -348,7 +402,13 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
return false;
}
Transform StereoCameraModel::transform() const
void StereoCameraModel::scale(double scale)
{
left_.scale(scale);
right_.scale(scale);
}
Transform StereoCameraModel::stereoTransform() const
{
if(!R_.empty() && !T_.empty())
{

17
corelib/src/CameraThread.cpp
View File

@@ -109,12 +109,12 @@ void CameraThread::mainLoop()
UDEBUG("");
cv::Mat rgb, depth;
float fx = 0.0f;
float fy = 0.0f;
float fyOrBaseline = 0.0f;
float cx = 0.0f;
float cy = 0.0f;
if(_cameraRGBD)
{
_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy);
_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy);
}
else
{
@@ -125,7 +125,18 @@ void CameraThread::mainLoop()
{
if(_cameraRGBD)
{
SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now());
SensorData data;
if(dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) || dynamic_cast<CameraStereoDC1394*>(_cameraRGBD))
{
//stereo
data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, UTimer::now());
UASSERT(data.stereoCameraModel().isValid());
}
else
{
data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, UTimer::now());
UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
}
this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
}
else

48
corelib/src/DBDriver.cpp
View File

@@ -412,15 +412,7 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
void DBDriver::getNodeData(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform,
int & laserScanMaxPts) const
SensorData & data) const
{
bool found = false;
// look in the trash
@@ -428,17 +420,9 @@ void DBDriver::getNodeData(
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
if(!s->getImageCompressed().empty() || !s->isSaved())
if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
{
imageCompressed = s->getImageCompressed();
depthCompressed = s->getDepthCompressed();
laserScanCompressed = s->getLaserScanCompressed();
fx = s->getFx();
fy = s->getFy();
cx = s->getCx();
cy = s->getCy();
localTransform = s->getLocalTransform();
laserScanMaxPts = s->getLaserScanMaxPts();
data = (SensorData)s->sensorData();
found = true;
}
}
@@ -447,31 +431,7 @@ void DBDriver::getNodeData(
if(!found)
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
_dbSafeAccessMutex.unlock();
}
}
void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
{
bool found = false;
// look in the trash
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
if(!s->getImageCompressed().empty() || !s->isSaved())
{
imageCompressed = s->getImageCompressed();
found = true;
}
}
_trashesMutex.unlock();
if(!found)
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed);
this->getNodeDataQuery(signatureId, data);
_dbSafeAccessMutex.unlock();
}
}

632
corelib/src/DBDriverSqlite3.cpp
View File

@@ -458,10 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(loadMetricData)
{
if(uStrNumCmp(_version, "0.8.11") >= 0)
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -471,7 +478,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -481,7 +488,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -491,10 +498,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
else
{
query << "SELECT data "
<< "FROM Image "
<< "WHERE id = ?"
<<";";
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else
{
query << "SELECT data "
<< "FROM Image "
<< "WHERE id = ?"
<<";";
}
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -519,13 +536,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
index = 0;
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
Transform localTransform = Transform::getIdentity();
cv::Mat scanCompressed;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the image
if(dataSize>4 && data)
{
(*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(loadMetricData)
@@ -534,35 +558,92 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
cv::Mat depthCompressed;
if(dataSize>4 && data)
{
depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.7.0") < 0)
if(uStrNumCmp(_version, "0.10.0") < 0)
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
(*iter)->setDepthCompressed(depthCompressed, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
float fx = sqlite3_column_double(ppStmt, index++);
float fy = sqlite3_column_double(ppStmt, index++);
float cx = sqlite3_column_double(ppStmt, index++);
float cy = sqlite3_column_double(ppStmt, index++);
(*iter)->setDepthCompressed(depthCompressed, fx, fy, cx, cy);
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform localTransform;
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
(*iter)->setLocalTransform(localTransform);
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
@@ -574,8 +655,30 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
//Create the laserScan
if(dataSize>4 && data)
{
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
if(models.size())
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
models,
(*iter)->id());
}
else
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
stereoModel,
(*iter)->id());
}
}
rc = sqlite3_step(ppStmt); // next result...
@@ -596,15 +699,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
void DBDriverSqlite3::getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform,
int & laserScanMaxPts) const
SensorData & sensorData) const
{
if(_ppDb)
{
@@ -614,10 +709,17 @@ void DBDriverSqlite3::getNodeDataQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.8.11") >= 0)
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan "
<< "FROM Data "
<< "WHERE id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -627,7 +729,7 @@ void DBDriverSqlite3::getNodeDataQuery(
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -637,7 +739,7 @@ void DBDriverSqlite3::getNodeDataQuery(
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -650,7 +752,15 @@ void DBDriverSqlite3::getNodeDataQuery(
const void * data = 0;
int dataSize = 0;
int index = 0;;
int index = 0;
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
Transform localTransform = Transform::getIdentity();
int laserScanMaxPts;
cv::Mat scanCompressed;
ULOGGER_DEBUG("Loading data for %d...", signatureId);
@@ -675,30 +785,88 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth image
if(dataSize>4 && data)
{
depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.7.0") < 0)
if(uStrNumCmp(_version, "0.10.0") < 0)
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
fx = 1.0f/depthConstant;
fy = 1.0f/depthConstant;
cx = 0.0f;
cy = 0.0f;
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras", cameraCount);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
dataFloat[i],
dataFloat[i+1],
dataFloat[i+2],
dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration for a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
fx = sqlite3_column_double(ppStmt, index++);
fy = sqlite3_column_double(ppStmt, index++);
cx = sqlite3_column_double(ppStmt, index++);
cy = sqlite3_column_double(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
laserScanMaxPts = 0;
@@ -712,63 +880,28 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth2d
if(dataSize>4 && data)
{
laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
if(models.size())
{
UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
sensorData = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
models,
signatureId);
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT data "
<< "FROM Image "
<< "WHERE id = " << signatureId
<<";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
const void * data = 0;
int dataSize = 0;
int index = 0;;
ULOGGER_DEBUG("Loading data for %d...", signatureId);
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
index = 0;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the image
if(dataSize>4 && data)
else
{
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
sensorData = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
stereoModel,
signatureId);
}
rc = sqlite3_step(ppStmt); // next result...
@@ -1900,7 +2033,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
const std::map<int, Link> & links = (*j)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
stepLink(ppStmt, i->second);
}
}
}
@@ -2008,7 +2141,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
const std::map<int, Link> & links = (*jter)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
stepLink(ppStmt, i->second);
}
}
// Finalize (delete) the statement
@@ -2048,40 +2181,66 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Add images
query = queryStepImage();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
if(!(*i)->getImageCompressed().empty())
// Add SensorData
query = queryStepSensorData();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
stepImage(ppStmt, (*i)->id(), (*i)->getImageCompressed());
if(!(*i)->sensorData().imageCompressed().empty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepSensorData(ppStmt, (*i)->sensorData());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Add depths
query = queryStepDepth();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
else
{
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
// Add images
query = queryStepImage();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
if(!(*i)->sensorData().imageCompressed().empty())
{
stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
// Add depths
query = queryStepDepth();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
//metric
if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepDepth(ppStmt, (*i)->sensorData());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
@@ -2216,12 +2375,14 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
std::string DBDriverSqlite3::queryStepImage() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
return "INSERT INTO Image(id, data) VALUES(?,?);";
}
void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
int id,
const cv::Mat & imageBytes) const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
if(!ppStmt)
{
@@ -2254,6 +2415,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepDepth() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
@@ -2267,18 +2429,13 @@ std::string DBDriverSqlite3::queryStepDepth() const
return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
}
}
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
int id,
const cv::Mat & depthBytes,
const cv::Mat & depth2dBytes,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
int depth2dMaxPts) const
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
{
UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
UDEBUG("Save depth %d (size=%d) depth2d = %d",
sensorData.id(),
(int)sensorData.depthOrRightCompressed().cols,
(int)sensorData.laserScanCompressed().cols);
if(!ppStmt)
{
UFATAL("");
@@ -2287,12 +2444,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, id);
rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(!depthBytes.empty())
if(!sensorData.depthOrRightCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, depthBytes.data, (int)depthBytes.cols, SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
}
else
{
@@ -2300,11 +2457,33 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
float fx=0, fyOrBaseline=0, cx=0, cy=0;
Transform localTransform = Transform::getIdentity();
if(sensorData.cameraModels().size())
{
UASSERT_MSG(sensorData.cameraModels().size() == 1,
uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
fx = sensorData.cameraModels()[0].fx();
fyOrBaseline = sensorData.cameraModels()[0].fy();
cx = sensorData.cameraModels()[0].cx();
cy = sensorData.cameraModels()[0].cy();
localTransform = sensorData.cameraModels()[0].localTransform();
}
else if(sensorData.stereoCameraModel().isValid())
{
fx = sensorData.stereoCameraModel().left().fx();
fyOrBaseline = sensorData.stereoCameraModel().baseline();
cx = sensorData.stereoCameraModel().left().cx();
cy = sensorData.stereoCameraModel().left().cy();
localTransform = sensorData.stereoCameraModel().left().localTransform();
}
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, fy);
rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, cx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2320,9 +2499,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(!depth2dBytes.empty())
if(!sensorData.laserScanCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, depth2dBytes.data, (int)depth2dBytes.cols, SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
}
else
{
@@ -2332,7 +2511,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
@@ -2344,6 +2523,116 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
}
void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
const SensorData & sensorData) const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
sensorData.id(),
(int)sensorData.imageCompressed().cols,
(int)sensorData.depthOrRightCompressed().cols,
(int)sensorData.laserScanCompressed().cols);
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
// id
rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// image
if(!sensorData.imageCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// depth or right image
if(!sensorData.depthOrRightCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// calibration
std::vector<float> calibration;
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(sensorData.cameraModels().size())
{
calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else if(sensorData.stereoCameraModel().isValid())
{
const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
calibration.resize(5+localTransform.size());
calibration[0] = sensorData.stereoCameraModel().left().fx();
calibration[1] = sensorData.stereoCameraModel().left().fy();
calibration[2] = sensorData.stereoCameraModel().left().cx();
calibration[3] = sensorData.stereoCameraModel().left().cy();
calibration[4] = sensorData.stereoCameraModel().baseline();
memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
}
if(calibration.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// scan
if(!sensorData.laserScanCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepLink() const
{
if(uStrNumCmp(_version, "0.8.4") >= 0)
@@ -2361,21 +2650,16 @@ std::string DBDriverSqlite3::queryStepLink() const
}
void DBDriverSqlite3::stepLink(
sqlite3_stmt * ppStmt,
int fromId,
int toId,
Link::Type type,
float rotVariance,
float transVariance,
const Transform & transform) const
const Link & link) const
{
if(!ppStmt)
{
UFATAL("");
}
UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
// Don't save virtual links
if(type==Link::kVirtualClosure)
if(link.type()==Link::kVirtualClosure)
{
UDEBUG("Virtual link ignored....");
return;
@@ -2383,27 +2667,27 @@ void DBDriverSqlite3::stepLink(
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, fromId);
rc = sqlite3_bind_int(ppStmt, index++, link.from());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, toId);
rc = sqlite3_bind_int(ppStmt, index++, link.to());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, type);
rc = sqlite3_bind_int(ppStmt, index++, link.type());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, transVariance);
rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, transform.data(), transform.size()*sizeof(float), SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);

29
corelib/src/DBDriverSqlite3.h
View File

@@ -71,18 +71,7 @@ private:
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform,
int & laserScanMaxPts) const;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
virtual void getNodeDataQuery(int signatureId, SensorData & data) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -94,6 +83,7 @@ private:
std::string queryStepNode() const;
std::string queryStepImage() const;
std::string queryStepDepth() const;
std::string queryStepSensorData() const;
std::string queryStepLink() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
@@ -102,18 +92,9 @@ private:
sqlite3_stmt * ppStmt,
int id,
const cv::Mat & imageBytes) const;
void stepDepth(
sqlite3_stmt * ppStmt,
int id,
const cv::Mat & depthBytes,
const cv::Mat & depth2dBytes,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
int depth2dMaxPts) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;

81
corelib/src/DBReader.cpp
View File

@@ -148,39 +148,39 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
SensorData data = this->getNextData();
if(data.isValid())
OdometryEvent odom = this->getNextData();
if(odom.isValid())
{
int goalId = 0;
double previousStamp = data.stamp();
data.setStamp(UTimer::now());
if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
double previousStamp = odom.data().stamp();
odom.data().setStamp(UTimer::now());
if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = uBytes2Str(data.userData());
std::string goalStr = uBytes2Str(odom.data().userData());
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = atoi(strs.rbegin()->c_str());
data.setUserData(std::vector<unsigned char>());
odom.data().setUserData(std::vector<unsigned char>());
}
}
}
if(!_odometryIgnored)
{
if(data.pose().isNull())
if(odom.pose().isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
this->post(new OdometryEvent(data));
this->post(new OdometryEvent(odom));
}
else
{
this->post(new CameraEvent(data));
this->post(new CameraEvent(odom.data()));
}
if(goalId > 0)
@@ -237,31 +237,26 @@ void DBReader::mainLoop()
}
SensorData DBReader::getNextData()
OdometryEvent DBReader::getNextData()
{
SensorData data;
OdometryEvent odom;
if(_dbDriver)
{
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat laserScanBytes;
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
int laserScanMaxPts = 0;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
SensorData data;
_dbDriver->getNodeData(*_currentId, data);
// info
Transform pose;
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
{
std::map<int, Link> links;
@@ -269,8 +264,7 @@ SensorData DBReader::getNextData()
if(links.size())
{
// assume the first is the backward neighbor, take its variance
rotVariance = links.begin()->second.rotVariance();
transVariance = links.begin()->second.transVariance();
infMatrix = links.begin()->second.infMatrix();
}
}
else
@@ -280,7 +274,7 @@ SensorData DBReader::getNextData()
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
if(data.imageCompressed().empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
@@ -334,33 +328,16 @@ SensorData DBReader::getNextData()
if(!this->isKilled())
{
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
laserScanMaxPts,
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
data.uncompressData();
data.setId(seq);
data.setStamp(stamp);
data.setUserData(userData);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1);
odom = OdometryEvent(data, pose, infMatrix.inv());
}
}
}
@@ -368,7 +345,7 @@ SensorData DBReader::getNextData()
{
UERROR("Not initialized...");
}
return data;
return odom;
}
} /* namespace rtabmap */

102
corelib/src/Graph.cpp
View File

@@ -260,20 +260,23 @@ std::map<int, Transform> TOROOptimizer::optimize(
AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
AISNavigation::TreePoseGraph2::InformationMatrix inf;
//Identity:
inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
if(!isCovarianceIgnored())
if(isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
}
inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
}
else
{
inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
int id1 = iter->first;
@@ -301,18 +304,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!isCovarianceIgnored())
{
if(iter->second.rotVariance()>0)
{
inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
}
if(iter->second.transVariance()>0)
{
inf[3][3] = 1.0f/iter->second.transVariance(); // x
inf[4][4] = 1.0f/iter->second.transVariance(); // y
inf[5][5] = 1.0f/iter->second.transVariance(); // z
}
memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
int id1 = iter->first;
@@ -476,7 +468,7 @@ bool TOROOptimizer::saveGraph(
{
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",
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(),
x,
@@ -485,12 +477,27 @@ bool TOROOptimizer::saveGraph(
roll,
pitch,
yaw,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f);
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(0,1),
iter->second.infMatrix().at<double>(0,2),
iter->second.infMatrix().at<double>(0,3),
iter->second.infMatrix().at<double>(0,4),
iter->second.infMatrix().at<double>(0,5),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(1,2),
iter->second.infMatrix().at<double>(1,3),
iter->second.infMatrix().at<double>(1,4),
iter->second.infMatrix().at<double>(1,5),
iter->second.infMatrix().at<double>(2,2),
iter->second.infMatrix().at<double>(2,3),
iter->second.infMatrix().at<double>(2,4),
iter->second.infMatrix().at<double>(2,5),
iter->second.infMatrix().at<double>(3,3),
iter->second.infMatrix().at<double>(3,4),
iter->second.infMatrix().at<double>(3,5),
iter->second.infMatrix().at<double>(4,4),
iter->second.infMatrix().at<double>(4,5),
iter->second.infMatrix().at<double>(5,5));
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);
@@ -674,15 +681,15 @@ std::map<int, Transform> G2OOptimizer::optimize(
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
information(2,2) = 1.0f/iter->second.rotVariance(); // theta
}
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
@@ -701,18 +708,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
information(2,2) = 1.0f/iter->second.transVariance(); // z
}
if(iter->second.rotVariance()>0)
{
information(3,3) = 1.0f/iter->second.rotVariance(); // roll
information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
}
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();

657
corelib/src/Memory.cpp
View File

File diff suppressed because it is too large Load Diff

15
corelib/src/Odometry.cpp
View File

@@ -89,16 +89,21 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
_pose.setIdentity(); // initialized
}
UASSERT(!data.image().empty());
UASSERT(!data.imageRaw().empty());
if(dynamic_cast<OdometryMono*>(this) == 0)
{
UASSERT(!data.depthOrRightImage().empty());
UASSERT(!data.depthOrRightRaw().empty());
}
if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
if(data.cameraModels().size() > 1)
{
UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
UERROR("Odometry doesn't support multi-camera yet.");
return Transform();
}
else if(!data.stereoCameraModel().isValid() &&
(data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
{
UERROR("Rectified images required! Calibrate your camera.");
return Transform();
}

18
corelib/src/OdometryBOW.cpp
View File

@@ -160,8 +160,15 @@ Transform OdometryBOW::computeTransform(
{
if(this->isPnPEstimationUsed())
{
if((int)newSignature->getWords().size() >= this->getMinInliers())
if(data.cameraModels().size() > 1)
{
UERROR("PnP cannot be used on multi-cameras setup.");
}
else if((int)newSignature->getWords().size() >= this->getMinInliers())
{
UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
std::vector<cv::Point3f> objectPoints(ids.size());
@@ -194,11 +201,8 @@ Transform OdometryBOW::computeTransform(
if((int)matches.size() >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()>0?data.fy():data.fx(), data.cy(),
0, 0, 1);
Transform guess = (this->getPose() * data.localTransform()).inverse();
cv::Mat K = cameraModel.K();
Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -229,7 +233,7 @@ Transform OdometryBOW::computeTransform(
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
transform = (data.localTransform() * pnp * this->getPose()).inverse();
transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse();
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());

23
corelib/src/OdometryICP.cpp
View File

@@ -72,25 +72,32 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
if(!data.depth().empty())
if(!data.depthOrRightRaw().empty())
{
if(data.depth().type() == CV_8UC1)
if(data.depthOrRightRaw().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
{
UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
return output;
}
const CameraModel & cameraModel = data.cameraModels()[0];
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
data.depthOrRightRaw(),
cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
data.localTransform());
cameraModel.localTransform());
if(_pointToPlane)
{

71
corelib/src/OdometryMono.cpp
View File

@@ -147,11 +147,24 @@ void OdometryMono::reset(const Transform & initialPose)
Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
{
UASSERT(!data.image().empty());
UASSERT(data.fx());
Transform output;
if(data.imageRaw().empty())
{
UERROR("Image empty! Cannot compute odometry...");
return output;
}
if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid())))
{
UERROR("Odometry cannot be done without calibration or on multi-camera!");
return output;
}
const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
UTimer timer;
Transform output;
int inliers = 0;
int correspondences = 0;
@@ -159,13 +172,13 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
cv::Mat newFrame;
// convert to grayscale
if(data.image().channels() > 1)
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.image().clone();
newFrame = data.imageRaw().clone();
}
if(memory_->getStMem().size() >= 1)
@@ -190,11 +203,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > this->getMinInliers())
{
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()==0?data.fx():data.fy(), data.cy(),
0, 0, 1);
Transform guess = (this->getPose() * data.localTransform()).inverse();
cv::Mat K = cameraModel.K();
Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -216,7 +226,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
UDEBUG("project points to previous image");
std::vector<cv::Point2f> prevImagePoints;
const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
cv::Mat prevR = (cv::Mat_<double>(3,3) <<
(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
@@ -240,8 +250,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
{
refCorners[oi] = prevImagePoints[i];
newCorners[oi] = imagePoints[i];
@@ -273,7 +283,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
prevS->getImageRaw(),
prevS->sensorData().imageRaw(),
newFrame,
refCorners,
newCorners,
@@ -357,7 +367,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
if(this->isInfoDataFilled() && info && inliersV.size())
{
@@ -402,9 +412,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
previousS->getWords(),
newS->getWords(),
data.fx(), data.fy()?data.fy():data.fx(),
data.cx(), data.cy(),
data.localTransform(),
cameraModel,
cameraTransform,
this->getIterations(),
this->getPnPReprojError(),
@@ -515,7 +523,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refS->getImageRaw(),
refS->sensorData().imageRaw(),
newFrame,
refCorners,
refCornersGuess,
@@ -652,10 +660,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
//UDEBUG("Correcting matches...done!");
UDEBUG("Computing P...");
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()==0?data.fx():data.fy(), data.cy(),
0, 0, 1);
cv::Mat K = cameraModel.K();
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
@@ -716,7 +721,15 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
(*inliersRef)[oi] = cloud->at(i);
if(!refDepth_.empty())
{
(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(
refDepth_,
refCorners[i].x,
refCorners[i].y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy(),
true);
}
++oi;
}
@@ -824,7 +837,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
output = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse();
if(output.getNorm() < minTranslation_*5)
{
reject = true;
@@ -844,7 +857,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
int index =inliersPnP.at(i);
int id = cornerIds[index];
UASSERT(id > 0 && id <= *wordsId.rbegin());
pcl::PointXYZ pt = util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
pcl::PointXYZ pt = util3d::transformPoint(
pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z),
this->getPose()*cameraModel.localTransform());
localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
keyFrameWords3D.insert(std::make_pair(id, pt));
}
@@ -890,7 +905,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
}
refDepth_ = data.depth().clone();
refDepth_ = data.depthOrRightRaw().clone();
keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
}
else

130
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -128,7 +128,7 @@ Transform OdometryOpticalFlow::computeTransform(
info->type = 1;
}
if(!data.rightImage().empty())
if(data.stereoCameraModel().isValid())
{
//stereo
return computeTransformStereo(data, info);
@@ -144,8 +144,13 @@ Transform OdometryOpticalFlow::computeTransformStereo(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.stereoCameraModel().isValid())
{
UERROR("Calibrated camera required.");
return output;
}
UTimer timer;
double variance = 0;
int inliers = 0;
@@ -153,15 +158,15 @@ Transform OdometryOpticalFlow::computeTransformStereo(
cv::Mat newLeftFrame;
// convert to grayscale
if(data.image().channels() > 1)
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
}
else
{
newLeftFrame = data.image().clone();
newLeftFrame = data.imageRaw().clone();
}
cv::Mat newRightFrame = data.rightImage().clone();
cv::Mat newRightFrame = data.depthOrRightRaw().clone();
std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
@@ -259,13 +264,16 @@ Transform OdometryOpticalFlow::computeTransformStereo(
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
if(pcl::isFinite(lastPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
lastPt3D = util3d::transformPoint(lastPt3D, data.stereoCameraModel().left().localTransform());
objectPoints[oi].x = lastPt3D.x;
objectPoints[oi].y = lastPt3D.y;
objectPoints[oi].z = lastPt3D.z;
@@ -278,11 +286,14 @@ Transform OdometryOpticalFlow::computeTransformStereo(
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
if(pcl::isFinite(newPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{
image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform());
image3DPoints[oi] = util3d::transformPoint(newPt3D, data.stereoCameraModel().left().localTransform());
}
}
@@ -313,11 +324,8 @@ Transform OdometryOpticalFlow::computeTransformStereo(
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fx(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat K = data.stereoCameraModel().left().K();
Transform guess = (data.stereoCameraModel().left().localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -348,7 +356,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
output = (data.stereoCameraModel().left().localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
@@ -416,18 +424,24 @@ Transform OdometryOpticalFlow::computeTransformStereo(
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
lastPt3D = util3d::transformPoint(lastPt3D, data.stereoCameraModel().left().localTransform());
newPt3D = util3d::transformPoint(newPt3D, data.stereoCameraModel().left().localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
if(this->isInfoDataFilled() && info)
@@ -566,8 +580,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid())
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
const CameraModel & cameraModel = data.cameraModels()[0];
UTimer timer;
double variance = 0;
int inliers = 0;
@@ -575,13 +595,13 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
cv::Mat newFrame;
// convert to grayscale
if(data.image().channels() > 1)
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.image().clone();
newFrame = data.imageRaw().clone();
}
std::vector<cv::Point2f> newCorners;
@@ -634,18 +654,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthOrRightRaw(),
newCorners[i].x,
newCorners[i].y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy(),
true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform());
image3DPoints[oi] = util3d::transformPoint(pt, cameraModel.localTransform());
}
}
@@ -676,11 +703,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat K = cameraModel.K();
Transform guess = (cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -711,7 +735,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
output = (cameraModel.localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
@@ -771,18 +795,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthOrRightRaw(),
newCorners[i].x,
newCorners[i].y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy(),
true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
pt = util3d::transformPoint(pt, cameraModel.localTransform());
correspondencesLast->at(oi) = refCorners3D_->at(i);
correspondencesNew->at(oi) = pt;
@@ -867,7 +898,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newFrame, roi);
Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
Feature2D::filterKeypointsByDepth(newKtps, data.depthOrRightRaw(), this->getMaxDepth());
if(newKtps.size())
{
@@ -892,18 +923,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
int oi=0;
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthOrRightRaw(),
newCorners[i].x,
newCorners[i].y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy(),
true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
pt = util3d::transformPoint(pt, cameraModel.localTransform());
newCorners3D->at(oi) = pt;
newCornersFiltered[oi] = newCorners[i];
++oi;

9
corelib/src/OdometryThread.cpp
View File

@@ -97,8 +97,9 @@ void OdometryThread::mainLoop()
{
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, info));
// a null pose notify that odometry could not be computed
double variance = info.variance>0?info.variance:1;
this->post(new OdometryEvent(data, pose, variance, variance, info));
}
}
@@ -106,7 +107,7 @@ void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
{
ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
return;
@@ -114,7 +115,7 @@ void OdometryThread::addData(const SensorData & data)
}
else
{
if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
return;

163
corelib/src/Rtabmap.cpp
View File

@@ -763,7 +763,10 @@ void Rtabmap::resetMemory()
//============================================================
// MAIN LOOP
//============================================================
bool Rtabmap::process(const SensorData & data)
bool Rtabmap::process(
const SensorData & data,
const Transform & odomPose,
const cv::Mat & covariance)
{
UDEBUG("");
@@ -821,11 +824,6 @@ bool Rtabmap::process(const SensorData & data)
// Wait for an image...
//============================================================
ULOGGER_INFO("getting data...");
if(!data.isValid())
{
ULOGGER_INFO("image is not valid...");
return false;
}
timer.start();
timerTotal.start();
@@ -839,7 +837,7 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
if(_rgbdSlamMode)
{
if(data.pose().isNull())
if(odomPose.isNull())
{
UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
"Image %d is ignored!", data.id());
@@ -853,7 +851,7 @@ bool Rtabmap::process(const SensorData & data)
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
// look for identity
if(!lastPose.isIdentity() && data.pose().isIdentity())
if(!lastPose.isIdentity() && odomPose.isIdentity())
{
int mapId = triggerNewMap();
UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
@@ -861,7 +859,7 @@ bool Rtabmap::process(const SensorData & data)
else if(_newMapOdomChangeDistance > 0.0)
{
// look for large change
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
@@ -871,7 +869,7 @@ bool Rtabmap::process(const SensorData & data)
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
odomPose.prettyPrint().c_str());
}
}
}
@@ -884,16 +882,14 @@ bool Rtabmap::process(const SensorData & data)
ULOGGER_INFO("Updating memory...");
if(_rgbdSlamMode)
{
if(!_memory->update(data, &statistics_))
if(!_memory->update(data, odomPose, covariance, &statistics_))
{
return false;
}
}
else
{
SensorData dataWithoutOdom = data;
dataWithoutOdom.setPose(Transform(), 1, 1);
if(!_memory->update(dataWithoutOdom, &statistics_))
if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
{
return false;
}
@@ -905,6 +901,7 @@ bool Rtabmap::process(const SensorData & data)
{
UFATAL("Not supposed to be here...last signature is null?!?");
}
ULOGGER_INFO("Processing signature %d", signature->id());
timeMemoryUpdate = timer.ticks();
ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
@@ -956,7 +953,7 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
if(_poseScanMatching &&
signature->getLinks().size() == 1 &&
!signature->getLaserScanCompressed().empty() &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
@@ -1039,7 +1036,7 @@ bool Rtabmap::process(const SensorData & data)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1584,16 +1581,13 @@ bool Rtabmap::process(const SensorData & data)
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true);
SensorData dataTo = tmpTo.toSensorData();
SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(dataFrom.isValid() &&
dataFrom.isMetric() &&
dataTo.isValid() &&
dataTo.isMetric() &&
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
tmpTo.id() != Memory::kIdInvalid)
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1629,7 +1623,7 @@ bool Rtabmap::process(const SensorData & data)
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
}
if(rejectedHypothesis)
@@ -1743,16 +1737,13 @@ bool Rtabmap::process(const SensorData & data)
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
Signature tmpTo = _memory->getSignatureData(nearestId, true);
SensorData dataTo = tmpTo.toSensorData();
SensorData dataTo = _memory->getNodeData(nearestId, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(dataFrom.isValid() &&
dataFrom.isMetric() &&
dataTo.isValid() &&
dataTo.isMetric() &&
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
tmpTo.id() != Memory::kIdInvalid)
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1784,7 +1775,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
nearestId,
transform.prettyPrint().c_str());
_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
if(_loopClosureHypothesis.first == 0)
{
@@ -1802,7 +1793,7 @@ bool Rtabmap::process(const SensorData & data)
//
// 2) compare locally with nearest locations by scan matching
//
if( !signature->getLaserScanCompressed().empty() &&
if( !signature->sensorData().laserScanCompressed().empty() &&
(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
{
// In localization mode, no need to check local loop
@@ -1873,7 +1864,7 @@ bool Rtabmap::process(const SensorData & data)
nearestId,
transform.prettyPrint().c_str());
// set Identify covariance for laser scan matching only
_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
++localSpaceClosuresAddedByICPOnly;
@@ -1913,6 +1904,7 @@ bool Rtabmap::process(const SensorData & data)
UINFO("Update map correction: SLAM mode");
// SLAM mode!
optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
// Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
@@ -1964,7 +1956,7 @@ bool Rtabmap::process(const SensorData & data)
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
{
_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance
_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance
}
}
}
@@ -2344,7 +2336,7 @@ bool Rtabmap::process(const SensorData & data)
bool Rtabmap::process(const cv::Mat & image, int id)
{
return this->process(SensorData(image, id));
return this->process(SensorData(image, id), Transform());
}
// SETTERS
@@ -2730,13 +2722,10 @@ void Rtabmap::dumpPrediction() const
}
}
void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
void Rtabmap::get3DMap(
std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global) const
{
@@ -2762,22 +2751,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
stamps.insert(std::make_pair(iter->first, stamp));
labels.insert(std::make_pair(iter->first, label));
userDatas.insert(std::make_pair(iter->first, userData));
}
// Get data
std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
@@ -2792,11 +2765,26 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
Signature data = _memory->getSignatureData(*iter);
if(data.id() != Memory::kIdInvalid)
{
signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
}
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
signatures.insert(std::make_pair(*iter,
Signature(*iter,
mapId,
weight,
stamp,
label,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
odomPose,
userData,
data)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -2812,12 +2800,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
void Rtabmap::getGraph(
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global)
bool global,
std::map<int, Signature> * signatures)
{
if(_memory && _memory->getLastWorkingSignature())
{
@@ -2840,19 +2825,29 @@ void Rtabmap::getGraph(
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
if(signatures)
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
stamps.insert(std::make_pair(iter->first, stamp));
labels.insert(std::make_pair(iter->first, label));
userDatas.insert(std::make_pair(iter->first, userData));
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
weight,
stamp,
label,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
odomPose,
userData,
SensorData())));
}
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -3002,11 +2997,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
if(computePath(targetNode, nodes, constraints))
@@ -3037,7 +3028,7 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
@@ -3189,7 +3180,7 @@ void Rtabmap::updateGoalIndex()
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}

68
corelib/src/RtabmapThread.cpp
View File

@@ -125,20 +125,13 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
_rtabmap->get3DMap(signatures,
poses,
constraints,
mapIds,
stamps,
labels,
userDatas,
optimized,
full);
this->post(new RtabmapEvent3DMap(signatures,
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints,
mapIds,
stamps,
labels,
userDatas));
constraints));
}
void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
@@ -153,20 +146,14 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
_rtabmap->getGraph(poses,
constraints,
mapIds,
stamps,
labels,
userDatas,
optimized,
full);
full,
&signatures);
this->post(new RtabmapEvent3DMap(signatures,
this->post(new RtabmapEvent3DMap(
signatures,
poses,
constraints,
mapIds,
stamps,
labels,
userDatas));
constraints));
}
@@ -301,7 +288,7 @@ void RtabmapThread::handleEvent(UEvent* event)
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
{
this->addData(e->data());
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
@@ -310,7 +297,7 @@ void RtabmapThread::handleEvent(UEvent* event)
OdometryEvent * e = (OdometryEvent*)event;
if(e->isValid())
{
this->addData(e->data());
this->addData(*e);
}
else
{
@@ -487,13 +474,13 @@ void RtabmapThread::handleEvent(UEvent* event)
//============================================================
void RtabmapThread::process()
{
SensorData data;
OdometryEvent data;
getData(data);
if(data.isValid() && _state.empty())
{
if(_rtabmap->getMemory())
{
if(_rtabmap->process(data))
if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
{
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -508,11 +495,11 @@ void RtabmapThread::process()
}
}
void RtabmapThread::addData(const SensorData & sensorData)
void RtabmapThread::addData(const OdometryEvent & odomEvent)
{
if(!_paused)
{
if(!sensorData.isValid())
if(!odomEvent.isValid())
{
ULOGGER_ERROR("data not valid !?");
return;
@@ -522,7 +509,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
{
if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
if(!lastPose_.isIdentity() && odomEvent.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
@@ -533,7 +520,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
return;
}
}
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity())
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
@@ -542,29 +529,30 @@ void RtabmapThread::addData(const SensorData & sensorData)
}
_frameRateTimer->start();
lastPose_ = sensorData.pose();
if(sensorData.poseRotVariance() > _rotVariance)
lastPose_ = odomEvent.pose();
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
if(maxRotVar > _rotVariance)
{
_rotVariance = sensorData.poseRotVariance();
_rotVariance = maxRotVar;
}
if(sensorData.poseTransVariance() > _transVariance)
if(maxTransVar > _transVariance)
{
_transVariance = sensorData.poseTransVariance();
_transVariance = maxTransVar;
}
bool notify = true;
_dataMutex.lock();
{
_dataBuffer.push_back(sensorData);
if(_rotVariance <= 0)
{
_rotVariance = 1.0f;
_rotVariance = 1.0;
}
if(_transVariance <= 0)
{
_transVariance = 1.0f;
_transVariance = 1.0;
}
_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
_rotVariance = 0;
_transVariance = 0;
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
@@ -583,7 +571,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
}
}
void RtabmapThread::getData(SensorData & image)
void RtabmapThread::getData(OdometryEvent & data)
{
ULOGGER_DEBUG("");
@@ -595,7 +583,7 @@ void RtabmapThread::getData(SensorData & image)
{
if(!_dataBuffer.empty())
{
image = _dataBuffer.front();
data = _dataBuffer.front();
_dataBuffer.pop_front();
}
}

503
corelib/src/SensorData.cpp
View File

@@ -27,138 +27,417 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UMath.h>
namespace rtabmap
{
/**
* An id is automatically generated if id=0.
*/
// empty constructor
SensorData::SensorData() :
_id(0),
_stamp(0.0),
_fx(0.0f),
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f),
_laserScanMaxPts(0)
_id(0),
_stamp(0.0),
_laserScanMaxPts(0)
{
}
SensorData::SensorData(const cv::Mat & image,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
_fx(0.0f),
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f),
_laserScanMaxPts(0),
_userData(userData)
// Appearance-only constructor
SensorData::SensorData(
const cv::Mat & image,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_userData(userData)
{
UASSERT(image.empty() ||
image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
if(image.rows == 1)
{
UASSERT(image.type() == CV_8UC1); // Bytes
_imageCompressed = image;
}
else if(!image.empty())
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
}
// Metric constructor
SensorData::SensorData(const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
float fyOrBaseline,
float cx,
float cy,
const Transform & localTransform,
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
_depthOrRightImage(depthOrRightImage),
_fx(fx),
_fyOrBaseline(fyOrBaseline),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(0),
_userData(userData)
// Mono constructor
SensorData::SensorData(
const cv::Mat & image,
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
{
UASSERT(image.empty() ||
image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
UASSERT(depthOrRightImage.empty() ||
depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!_localTransform.isNull());
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
if(image.rows == 1)
{
UASSERT(image.type() == CV_8UC1); // Bytes
_imageCompressed = image;
}
else if(!image.empty())
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
}
// Metric constructor + 2d depth
SensorData::SensorData(const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
float fyOrBaseline,
float cx,
float cy,
const Transform & localTransform,
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
_depthOrRightImage(depthOrRightImage),
_laserScan(laserScan),
_fx(fx),
_fyOrBaseline(fyOrBaseline),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance),
_laserScanMaxPts(laserScanMaxPts),
_userData(userData)
// RGB-D constructor
SensorData::SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
UASSERT(image.empty() ||
image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
UASSERT(depthOrRightImage.empty() ||
depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!_localTransform.isNull());
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
if(rgb.rows == 1)
{
UASSERT(rgb.type() == CV_8UC1); // Bytes
_imageCompressed = rgb;
}
else if(!rgb.empty())
{
UASSERT(rgb.type() == CV_8UC1 || // Mono
rgb.type() == CV_8UC3); // RGB
_imageRaw = rgb;
}
if(depth.rows == 1)
{
UASSERT(depth.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = depth;
}
else if(!depth.empty())
{
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
}
bool SensorData::empty() const
// RGB-D constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
{
return _image.empty();
if(rgb.rows == 1)
{
UASSERT(rgb.type() == CV_8UC1); // Bytes
_imageCompressed = rgb;
}
else if(!rgb.empty())
{
UASSERT(rgb.type() == CV_8UC1 || // Mono
rgb.type() == CV_8UC3); // RGB
_imageRaw = rgb;
}
if(depth.rows == 1)
{
UASSERT(depth.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = depth;
}
else if(!depth.empty())
{
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
if(laserScan.rows == 1)
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
else if(!laserScan.empty())
{
UASSERT(laserScan.type() == CV_32FC2);
_laserScanRaw = laserScan;
}
}
// Multi-cameras RGB-D constructor
SensorData::SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(cameraModels),
_userData(userData)
{
if(rgb.rows == 1)
{
UASSERT(rgb.type() == CV_8UC1); // Bytes
_imageCompressed = rgb;
}
else if(!rgb.empty())
{
UASSERT(rgb.type() == CV_8UC1 || // Mono
rgb.type() == CV_8UC3); // RGB
_imageRaw = rgb;
}
if(depth.rows == 1)
{
UASSERT(depth.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = depth;
}
else if(!depth.empty())
{
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
UASSERT(cameraModels[i].isValid());
}
}
// Multi-cameras RGB-D constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(cameraModels),
_userData(userData)
{
if(rgb.rows == 1)
{
UASSERT(rgb.type() == CV_8UC1); // Bytes
_imageCompressed = rgb;
}
else if(!rgb.empty())
{
UASSERT(rgb.type() == CV_8UC1 || // Mono
rgb.type() == CV_8UC3); // RGB
_imageRaw = rgb;
}
if(depth.rows == 1)
{
UASSERT(depth.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = depth;
}
else if(!depth.empty())
{
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
if(laserScan.rows == 1)
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
else if(!laserScan.empty())
{
UASSERT(laserScan.type() == CV_32FC2);
_laserScanRaw = laserScan;
}
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
UASSERT(cameraModels[i].isValid());
}
}
// Stereo constructor
SensorData::SensorData(
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData):
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_stereoCameraModel(cameraModel),
_userData(userData)
{
if(left.rows == 1)
{
UASSERT(left.type() == CV_8UC1); // Bytes
_imageCompressed = left;
}
else if(!left.empty())
{
UASSERT(left.type() == CV_8UC1 || // Mono
left.type() == CV_8UC3); // RGB
_imageRaw = left;
}
if(right.rows == 1)
{
UASSERT(right.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = right;
}
else if(!right.empty())
{
UASSERT(right.type() == CV_8UC1); // Mono
_depthOrRightRaw = right;
}
}
// Stereo constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_stereoCameraModel(cameraModel),
_userData(userData)
{
if(left.rows == 1)
{
UASSERT(left.type() == CV_8UC1); // Bytes
_imageCompressed = left;
}
else if(!left.empty())
{
UASSERT(left.type() == CV_8UC1 || // Mono
left.type() == CV_8UC3); // RGB
_imageRaw = left;
}
if(right.rows == 1)
{
UASSERT(right.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = right;
}
else if(!right.empty())
{
UASSERT(right.type() == CV_8UC1); // Mono
_depthOrRightRaw = right;
}
if(laserScan.rows == 1)
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
else if(!laserScan.empty())
{
UASSERT(laserScan.type() == CV_32FC2);
_laserScanRaw = laserScan;
}
}
void SensorData::uncompressData()
{
uncompressData(&_imageRaw, &_depthOrRightRaw, &_laserScanRaw);
}
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
{
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
}
if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
{
_depthOrRightRaw = *depthRaw;
}
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
{
_laserScanRaw = *laserScanRaw;
}
}
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
{
if(imageRaw)
{
*imageRaw = _imageRaw;
}
if(depthRaw)
{
*depthRaw = _depthOrRightRaw;
}
if(laserScanRaw)
{
*laserScanRaw = _laserScanRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
}
if(depthRaw && depthRaw->empty())
{
ctDepth.start();
}
if(laserScanRaw && laserScanRaw->empty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
}
if(depthRaw && depthRaw->empty())
{
*depthRaw = ctDepth.getUncompressedData();
}
if(laserScanRaw && laserScanRaw->empty())
{
*laserScanRaw = ctLaserScan.getUncompressedData();
}
}
}
} // namespace rtabmap

152
corelib/src/Signature.cpp
View File

@@ -44,12 +44,7 @@ Signature::Signature() :
_saved(false),
_modified(true),
_linksModified(true),
_enabled(false),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_laserScanMaxPts(0)
_enabled(false)
{
}
@@ -63,15 +58,7 @@ Signature::Signature(
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const std::vector<unsigned char> & userData,
const cv::Mat & laserScanCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
int laserScanMaxPts) :
const SensorData & sensorData) :
_id(id),
_mapId(mapId),
_stamp(stamp),
@@ -82,18 +69,10 @@ Signature::Signature(
_modified(true),
_linksModified(true),
_words(words),
_enabled(false),
_imageCompressed(imageCompressed),
_depthCompressed(depthCompressed),
_laserScanCompressed(laserScanCompressed),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform),
_words3(words3),
_laserScanMaxPts(laserScanMaxPts)
_enabled(false),
_pose(pose),
_sensorData(sensorData)
{
}
@@ -239,25 +218,9 @@ void Signature::removeWord(int wordId)
_words3.erase(wordId);
}
void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
cv::Mat Signature::getPoseCovariance() const
{
UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
_depthCompressed = bytes;
_fx=fx;
_fy=fy;
_cx=cx;
_cy=cy;
}
float Signature::getDepthFx() const {return getFx();}
float Signature::getDepthFy() const {return getFy();}
float Signature::getDepthCx() const {return getCx();}
float Signature::getDepthCy() const {return getCy();}
void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
{
rotVariance = 1.0f;
transVariance = 1.0f;
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(_links.size())
{
for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -267,110 +230,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons
//Assume the first neighbor to be the backward neighbor link
if(iter->second.to() < iter->second.from())
{
rotVariance = iter->second.rotVariance();
transVariance = iter->second.transVariance();
covariance = iter->second.infMatrix().inv();
break;
}
}
}
}
}
SensorData Signature::toSensorData()
{
this->uncompressData();
float rotVariance = 1.0f;
float transVariance = 1.0f;
this->getPoseVariance(rotVariance, transVariance);
return SensorData(_laserScanRaw,
_laserScanMaxPts,
_imageRaw,
_depthRaw,
_fx,
_fy,
_cx,
_cy,
_localTransform,
_pose,
rotVariance,
transVariance,
_id,
_stamp,
_userData);
}
void Signature::uncompressData()
{
uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
}
void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
{
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
}
if(depthRaw && !depthRaw->empty() && _depthRaw.empty())
{
_depthRaw = *depthRaw;
}
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
{
_laserScanRaw = *laserScanRaw;
}
}
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
{
if(imageRaw)
{
*imageRaw = _imageRaw;
}
if(depthRaw)
{
*depthRaw = _depthRaw;
}
if(laserScanRaw)
{
*laserScanRaw = _laserScanRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
}
if(depthRaw && depthRaw->empty())
{
ctDepth.start();
}
if(laserScanRaw && laserScanRaw->empty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
}
if(depthRaw && depthRaw->empty())
{
*depthRaw = ctDepth.getUncompressedData();
}
if(laserScanRaw && laserScanRaw->empty())
{
*laserScanRaw = ctLaserScan.getUncompressedData();
}
}
return covariance;
}
} //namespace rtabmap

23
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -25,24 +25,13 @@ CREATE TABLE Node (
PRIMARY KEY (id)
);
CREATE TABLE Image (
CREATE TABLE Data (
id INTEGER NOT NULL,
data BLOB, -- compressed image (RGB)
time_enter DATE,
PRIMARY KEY (id)
);
-- TODO: Merge "Image" and "Depth" tables to "Data" table.
CREATE TABLE Depth (
id INTEGER NOT NULL,
data BLOB, -- compressed image (Depth or Right image)
fx FLOAT,
fy FLOAT, -- baseline if stereo
cx FLOAT,
cy FLOAT,
local_transform BLOB,
data2d BLOB, -- compressed data (Laser scan)
data2d_max_pts INTEGER, -- Laser scan max points
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform
scan BLOB, -- compressed data (Laser scan)
scan_max_pts INTEGER, -- Laser scan max points
time_enter DATE,
PRIMARY KEY (id)
);

241
corelib/src/util3d.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
@@ -482,6 +483,246 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
decimation);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
float voxelSize,
int samples)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
//depth
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValid())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
tmp = util3d::voxelize(tmp, voxelSize);
filtered = true;
}
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
filtered = true;
}
if(tmp->size() && !filtered)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
}
}
else
{
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
}
}
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
{
//stereo
UASSERT(sensorData.rightRaw().type() == CV_8UC1);
cv::Mat leftMono;
if(sensorData.imageRaw().channels() == 3)
{
cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
}
else
{
leftMono = sensorData.imageRaw();
}
return cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
decimation);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
}
}
return cloud;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
const SensorData & sensorData,
int decimation,
float maxDepth,
float voxelSize,
int samples)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(!sensorData.imageRaw().empty())
{
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
//depth
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValid())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
tmp = util3d::voxelize(tmp, voxelSize);
filtered = true;
}
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
filtered = true;
}
if(tmp->size() && !filtered)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
}
}
else
{
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
}
}
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
{
//stereo
cloud = cloudFromStereoImages(sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
decimation);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
}
}
}
return cloud;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,

96
corelib/src/util3d_features.cpp
View File

@@ -44,36 +44,48 @@ namespace rtabmap
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel)
{
UASSERT(cameraModel.isValid());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & transform)
const std::vector<CameraModel> & cameraModels)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
UASSERT(cameraModels.size());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
if(!depth.empty())
{
UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
float subImageWidth = depth.cols/cameraModels.size();
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
UASSERT(cameraIndex < (int)cameraModels.size());
pcl::PointXYZ pt = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x,
keypoints[i].pt.x-subImageWidth*cameraIndex,
keypoints[i].pt.y,
cx,
cy,
fx,
fy,
cameraModels.at(cameraIndex).cx(),
cameraModels.at(cameraIndex).cy(),
cameraModels.at(cameraIndex).fx(),
cameraModels.at(cameraIndex).fy(),
true);
if(!transform.isNull() && !transform.isIdentity())
if(!cameraModels.at(cameraIndex).localTransform().isNull() &&
!cameraModels.at(cameraIndex).localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, transform);
pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
}
keypoints3d->at(i) = pt;
}
@@ -84,13 +96,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform)
const StereoCameraModel & stereoCameraModel)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
UASSERT(stereoCameraModel.isValid());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
@@ -98,14 +107,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
cx,
cy,
fx,
baseline);
stereoCameraModel.left().cx(),
stereoCameraModel.left().cy(),
stereoCameraModel.left().fx(),
stereoCameraModel.baseline());
if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
if(pcl::isFinite(pt) &&
!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, transform);
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
}
keypoints3d->at(i) = pt;
}
@@ -116,11 +127,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform,
const StereoCameraModel & stereoCameraModel,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
@@ -129,6 +136,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
UASSERT(stereoCameraModel.isValid());
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
@@ -165,14 +173,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
cx, cy, fx, baseline);
stereoCameraModel.left().cx(),
stereoCameraModel.left().cy(),
stereoCameraModel.left().fx(),
stereoCameraModel.baseline());
if(pcl::isFinite(tmpPt))
{
pt = tmpPt;
if(!transform.isNull() && !transform.isIdentity())
if(!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, transform);
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
}
}
}
@@ -190,11 +202,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
std::multimap<int, pcl::PointXYZ> generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & refWords,
const std::multimap<int, cv::KeyPoint> & nextWords,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
const CameraModel & cameraModel,
Transform & cameraTransform,
int pnpIterations,
float pnpReprojError,
@@ -204,6 +212,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
const std::multimap<int, pcl::PointXYZ> & refGuess3D,
double * varianceOut)
{
UASSERT(cameraModel.isValid());
std::multimap<int, pcl::PointXYZ> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
@@ -257,10 +266,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
xp.at<double>(2, i) = 1;
}
cv::Mat K = (cv::Mat_<double>(3,3) <<
fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Mat K = cameraModel.K();
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
cv::Mat x_norm = Kinv * x;
@@ -280,7 +286,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
//if camera transform is set, use it instead of the computed one from epipolar geometry
if(useCameraTransformGuess)
{
Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
P = (cv::Mat_<double>(3,4) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
@@ -303,7 +309,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
pts4D.col(i) /= pts4D.at<double>(3,i);
if(pts4D.at<double>(2,i) > 0)
{
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
}
}
@@ -316,7 +322,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (localTransform * t).inverse() * localTransform;
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
}
if(refGuess3D.size())
@@ -408,7 +414,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
imagePoints.resize(oi);
//PnPRansac
Transform guess = localTransform.inverse();
Transform guess = cameraModel.localTransform().inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -440,7 +446,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
cameraTransform = (localTransform * pnp).inverse();
cameraTransform = (cameraModel.localTransform() * pnp).inverse();
}
else
{