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

DbDriverSqlite3: Fixed multi-camera calibration loading error with version >= 0.11.2

Browse Source
This commit is contained in:
matlabbe
2016-06-16 14:57:55 -04:00
parent 798c3cb373
commit 62a982ef9f
1 changed files with 108 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
corelib/src/DBDriverSqlite3.cpp
View File

@@ -882,13 +882,39 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
if(dataSize > 0 && data) if(dataSize > 0 && data)
{ {
float * dataFloat = (float*)data; float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{ {
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size()); int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size()) for(int i=0; i<max; i+=4+localTransform.size())
{ {
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float)); memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel( models.push_back(CameraModel(
(double)dataFloat[i], (double)dataFloat[i],
@@ -910,26 +936,6 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
dataFloat[4], // baseline dataFloat[4], // baseline
localTransform); localTransform);
} }
else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else else
{ {
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize); UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1103,13 +1109,39 @@ bool DBDriverSqlite3::getCalibrationQuery(
if(dataSize > 0 && data) if(dataSize > 0 && data)
{ {
float * dataFloat = (float*)data; float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{ {
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size()); int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size()) for(int i=0; i<max; i+=4+localTransform.size())
{ {
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float)); memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel( models.push_back(CameraModel(
(double)dataFloat[i], (double)dataFloat[i],
@@ -1131,26 +1163,6 @@ bool DBDriverSqlite3::getCalibrationQuery(
dataFloat[4], // baseline dataFloat[4], // baseline
localTransform); localTransform);
} }
else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else else
{ {
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize); UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1897,7 +1909,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int index=0; int index=0;
const void * data = 0; const void * data = 0;
int dataSize = 0; int dataSize = 0;
Transform localTransform = Transform::getIdentity(); Transform localTransform;
std::vector<CameraModel> models; std::vector<CameraModel> models;
StereoCameraModel stereoModel; StereoCameraModel stereoModel;
@@ -1910,13 +1922,39 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{ {
++calibrationsLoaded; ++calibrationsLoaded;
float * dataFloat = (float*)data; float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{ {
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size()); int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size()) for(int i=0; i<max; i+=4+localTransform.size())
{ {
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float)); memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel( models.push_back(CameraModel(
(double)dataFloat[i], (double)dataFloat[i],
@@ -1938,26 +1976,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
dataFloat[4], // baseline dataFloat[4], // baseline
localTransform); localTransform);
} }
else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else else
{ {
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize); UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -3218,17 +3236,33 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(sensorData.cameraModels().size()) if(sensorData.cameraModels().size())
{ {
calibration.resize(sensorData.cameraModels().size() * (6+Transform().size())); if(uStrNumCmp(_version, "0.11.2") >= 0)
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{ {
const Transform & localTransform = sensorData.cameraModels()[i].localTransform(); calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx(); for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy(); {
calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx(); const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy(); calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth(); calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight(); calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float)); calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else
{
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().isValidForProjection()) else if(sensorData.stereoCameraModel().isValidForProjection())