i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master to devel

Browse Source
This commit is contained in:
matlabbe
2016-07-04 15:38:28 -04:00
parent 02d4ca5c8b 9dfc7801a0
commit 1365eaca3a
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBDriverSqlite3.cpp
+5 -3
View File
@@ -3243,13 +3243,14 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
std::vector<float> calibration; std::vector<float> calibration;
// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras // multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
// 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() && sensorData.cameraModels()[0].isValidForProjection())
{ {
if(uStrNumCmp(_version, "0.11.2") >= 0) if(uStrNumCmp(_version, "0.11.2") >= 0)
{ {
calibration.resize(sensorData.cameraModels().size() * (6+Transform().size())); calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i) for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{ {
UASSERT(sensorData.cameraModels()[i].isValidForProjection());
const Transform & localTransform = sensorData.cameraModels()[i].localTransform(); const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx(); calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy(); calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
@@ -3265,13 +3266,14 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
calibration.resize(sensorData.cameraModels().size() * (4+Transform().size())); calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i) for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{ {
UASSERT(sensorData.cameraModels()[i].isValidForProjection());
const Transform & localTransform = sensorData.cameraModels()[i].localTransform(); const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx(); calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy(); 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())+2] = sensorData.cameraModels()[i].cx();
calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy(); calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float)); memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
} }
} }
} }
else if(sensorData.stereoCameraModel().isValidForProjection()) else if(sensorData.stereoCameraModel().isValidForProjection())