i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.18: Camera calibration and LaserScan Info refactoring (#324)

Browse Source 
* Saving full camera calibration in database, added angle min/max/inc to LaserScan.

* Updated laserscan info save/load in db

* Database: added Tag table, added env_sensors field to Node

* fixed serialization/deserialization of stereo camera model

* fixed multi-calibration db saving

* fixed rebase errors

* Tango: Added saving environmental sensors option

* Memory: Save env sensors

* Tango: fixed env sensor ids

* DBViewer: show env sensors values

* DBViewer: added calibration details on tooltip

* increased package version to 0.18.0

* Fixed LaserScan copies when angleIncrement is valid

* fixed build error without OctoMap dependency
This commit is contained in:
matlabbe
2018-10-23 14:35:14 -04:00
committed by GitHub
parent 8701ae6de0
commit 8e99291e13
51 changed files with 2063 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraModel.cpp
+119
View File
@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
@@ -449,6 +450,124 @@ bool CameraModel::save(const std::string & directory) const
return false;
}
std::vector<unsigned char> CameraModel::serialize() const
{
const int headerSize = 11;
int header[headerSize] = {
RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
0, //mono // 3,
imageSize_.width, imageSize_.height, // 4,5
(int)K_.total(), (int)D_.total(), (int)R_.total(), (int)P_.total(), // 6,7,8,9
localTransform_.isNull()?0:localTransform_.size()}; // 10
UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
std::vector<unsigned char> data(
sizeof(int)*headerSize +
sizeof(double)*(K_.total()+D_.total()+R_.total()+P_.total()) +
(localTransform_.isNull()?0:sizeof(float)*localTransform_.size()));
memcpy(data.data(), header, sizeof(int)*headerSize);
int index = sizeof(int)*headerSize;
if(!K_.empty())
{
memcpy(data.data()+index, K_.data, sizeof(double)*(K_.total()));
index+=sizeof(double)*(K_.total());
}
if(!D_.empty())
{
memcpy(data.data()+index, D_.data, sizeof(double)*(D_.total()));
index+=sizeof(double)*(D_.total());
}
if(!R_.empty())
{
memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
index+=sizeof(double)*(R_.total());
}
if(!P_.empty())
{
memcpy(data.data()+index, P_.data, sizeof(double)*(P_.total()));
index+=sizeof(double)*(P_.total());
}
if(!localTransform_.isNull())
{
memcpy(data.data()+index, localTransform_.data(), sizeof(float)*(localTransform_.size()));
index+=sizeof(float)*(localTransform_.size());
}
return data;
}
unsigned int CameraModel::deserialize(const std::vector<unsigned char>& data)
{
return deserialize(data.data(), data.size());
}
unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
{
*this = CameraModel();
int headerSize = 11;
if(dataSize >= sizeof(int)*headerSize)
{
UASSERT(data != 0);
const int * header = (const int *)data;
int type = header[3];
if(type == 0)
{
imageSize_.width = header[4];
imageSize_.height = header[5];
int iK = 6;
int iD = 7;
int iR = 8;
int iP = 9;
int iL = 10;
UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
unsigned int requiredDataSize = sizeof(int)*headerSize +
sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
sizeof(float)*header[iL];
UASSERT_MSG(dataSize >= requiredDataSize,
uFormat("dataSize=%d != required=%d (header: version %d.%d.%d %dx%d type=%d K=%d D=%d R=%d P=%d L=%d)",
dataSize,
requiredDataSize,
header[0], header[1], header[2], header[4], header[5], header[3],
header[iK], header[iD], header[iR],header[iP], header[iL]).c_str());
unsigned int index = sizeof(int)*headerSize;
if(header[iK] != 0)
{
UASSERT(header[iK] == 9);
K_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(K_.total());
}
if(header[iD] != 0)
{
D_ = cv::Mat(1, header[iD], CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(D_.total());
}
if(header[iR] != 0)
{
UASSERT(header[iR] == 9);
R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(R_.total());
}
if(header[iP] != 0)
{
UASSERT(header[iP] == 12);
P_ = cv::Mat(3, 4, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(P_.total());
}
if(header[iL] != 0)
{
UASSERT(header[iL] == 12);
memcpy(localTransform_.data(), data+index, sizeof(float)*localTransform_.size());
index+=sizeof(float)*localTransform_.size();
}
UASSERT(index <= dataSize);
return index;
}
else
{
UERROR("Serialized calibration is not mono (type=%d), use the appropriate class matching the type to deserialize.", type);
}
}
UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
return 0;
}
CameraModel CameraModel::scaled(double scale) const
{
CameraModel scaledModel = *this;