i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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

119
corelib/src/CameraModel.cpp
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;

33
corelib/src/DBDriver.cpp
View File

@@ -733,7 +733,8 @@ bool DBDriver::getNodeInfo(
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity,
GPS & gps) const
GPS & gps,
EnvSensors & sensors) const
{
bool found = false;
// look in the trash
@@ -747,6 +748,7 @@ bool DBDriver::getNodeInfo(
stamp = _trashSignatures.at(signatureId)->getStamp();
groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
gps = _trashSignatures.at(signatureId)->sensorData().gps();
sensors = _trashSignatures.at(signatureId)->sensorData().envSensors();
found = true;
}
_trashesMutex.unlock();
@@ -754,7 +756,7 @@ bool DBDriver::getNodeInfo(
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps);
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps, sensors);
_dbSafeAccessMutex.unlock();
}
return found;
@@ -790,6 +792,33 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
}
}
void DBDriver::loadTags(int signatureId, std::map<int, TransformStamped> & tags) const
{
bool found = false;
// look in the trash
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
UASSERT(s != 0);
for(std::map<int, TransformStamped>::const_iterator nIter = s->getTags().begin();
nIter!=s->getTags().end();
++nIter)
{
tags.insert(*nIter);
}
found = true;
}
_trashesMutex.unlock();
if(!found)
{
_dbSafeAccessMutex.lock();
this->loadTagsQuery(signatureId, tags);
_dbSafeAccessMutex.unlock();
}
}
void DBDriver::getWeight(int signatureId, int & weight) const
{
bool found = false;

1072
corelib/src/DBDriverSqlite3.cpp
View File

File diff suppressed because it is too large Load Diff

7
corelib/src/DBReader.cpp
View File

@@ -268,7 +268,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
Transform localTransform, pose, groundTruth;
std::vector<float> velocity;
GPS gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
EnvSensors sensors;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
@@ -323,7 +324,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
Transform groundTruth;
std::vector<float> velocity;
GPS gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
EnvSensors sensors;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
@@ -437,6 +439,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.setStamp(stamp);
data.setGroundTruth(groundTruth);
data.setGPS(gps);
data.setEnvSensors(sensors);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().isEmpty()?0:1,
data.imageRaw().empty()?0:1,

131
corelib/src/LaserScan.cpp
View File

@@ -81,7 +81,11 @@ bool LaserScan::isScanHasIntensity(const Format & format)
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
}
LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints, int maxRange, const Transform & localTransform)
LaserScan LaserScan::backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints,
int maxRange,
const Transform & localTransform)
{
if(!oldScanFormat.empty())
{
@@ -113,19 +117,71 @@ LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int ma
return LaserScan();
}
LaserScan LaserScan::backwardCompatibility(
const cv::Mat & oldScanFormat,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleInc,
const Transform & localTransform)
{
if(!oldScanFormat.empty())
{
if(oldScanFormat.channels() == 2)
{
return LaserScan(oldScanFormat, kXY, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else if(oldScanFormat.channels() == 3)
{
return LaserScan(oldScanFormat, kXYZ, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else if(oldScanFormat.channels() == 4)
{
return LaserScan(oldScanFormat, kXYZRGB, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else if(oldScanFormat.channels() == 5)
{
return LaserScan(oldScanFormat, kXYNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else if(oldScanFormat.channels() == 6)
{
return LaserScan(oldScanFormat, kXYZNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
else if(oldScanFormat.channels() == 7)
{
return LaserScan(oldScanFormat, kXYZRGBNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
}
}
return LaserScan();
}
LaserScan::LaserScan() :
maxPoints_(0),
maxRange_(0),
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0),
localTransform_(Transform::getIdentity())
{
}
LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform) :
LaserScan::LaserScan(
const cv::Mat & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform) :
data_(data),
maxPoints_(maxPoints),
maxRange_(maxRange),
format_(format),
maxPoints_(maxPoints),
rangeMin_(0),
rangeMax_(maxRange),
angleMin_(0),
angleMax_(0),
angleIncrement_(0),
localTransform_(localTransform)
{
UASSERT(data.empty() || data.rows == 1);
@@ -136,7 +192,7 @@ LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format
{
if(format == kUnknown)
{
*this = backwardCompatibility(data_, maxPoints_, maxRange_, localTransform_);
*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
}
else // verify that format corresponds to expected number of channels
{
@@ -150,4 +206,65 @@ LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format
}
}
LaserScan::LaserScan(
const cv::Mat & data,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform) :
data_(data),
format_(format),
rangeMin_(minRange),
rangeMax_(maxRange),
angleMin_(angleMin),
angleMax_(angleMax),
angleIncrement_(angleIncrement),
localTransform_(localTransform)
{
UASSERT(maxRange>minRange);
UASSERT(angleMax>angleMin);
UASSERT(angleIncrement != 0.0f);
maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement);
UASSERT(data.empty() || data.rows == 1);
UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(!localTransform.isNull());
if(!data.empty() && !isCompressed())
{
if(data_.cols > maxPoints_)
{
UWARN("The number of points (%d) in the scan is over the maximum "
"points (%d) defined by angle settings (min=%f max=%f inc=%f). "
"The scan info may be wrong!",
data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
}
if(format == kUnknown)
{
*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
}
else // verify that format corresponds to expected number of channels
{
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
}
}
}
LaserScan LaserScan::clone() const
{
if(angleIncrement_ > 0.0f)
{
return LaserScan(data_.clone(), format_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_.clone());
}
return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
}
}

84
corelib/src/Memory.cpp
View File

@@ -2796,11 +2796,11 @@ Transform Memory::computeIcpTransformMulti(
{
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
float guessNorm = guess.getNorm();
if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
guessNorm > fromScan.maxRange() + toScan.maxRange())
if(fromScan.rangeMax() > 0.0f && toScan.rangeMax() > 0.0f &&
guessNorm > fromScan.rangeMax() + toScan.rangeMax())
{
// stop right known,it is impossible that scans overlay.
UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
return t;
}
@@ -2890,7 +2890,7 @@ Transform Memory::computeIcpTransformMulti(
assembledData.setLaserScanRaw(
LaserScan(assembledScan,
fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
fromScan.maxRange(),
fromScan.rangeMax(),
fromScan.format(),
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
@@ -3430,7 +3430,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
double stamp;
std::vector<float> velocity;
GPS gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
EnvSensors sensors;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
return pose;
}
@@ -3442,7 +3443,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
double stamp;
std::vector<float> velocity;
GPS gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
EnvSensors sensors;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
return groundTruth;
}
@@ -3456,7 +3458,8 @@ void Memory::getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabas
std::string label;
double stamp;
std::vector<float> velocity;
getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
EnvSensors sensors;
getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
if(gps.stamp() == 0.0)
{
@@ -3502,6 +3505,7 @@ bool Memory::getNodeInfo(int signatureId,
Transform & groundTruth,
std::vector<float> & velocity,
GPS & gps,
EnvSensors & sensors,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
@@ -3515,11 +3519,12 @@ bool Memory::getNodeInfo(int signatureId,
groundTruth = s->getGroundTruthPose();
velocity = s->getVelocity();
gps = s->sensorData().gps();
sensors = s->sensorData().envSensors();
return true;
}
else if(lookInDatabase && _dbDriver)
{
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps);
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
}
return false;
}
@@ -4451,7 +4456,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
LaserScan laserScan = data.laserScanRaw();
if(!isIntermediateNode && laserScan.size())
{
if(laserScan.maxRange() == 0.0f)
if(laserScan.rangeMax() == 0.0f)
{
bool id2d = laserScan.is2d();
float maxRange = 0.0f;
@@ -4561,7 +4566,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
SensorData(
LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
laserScan.angleMin(),
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
compressedImage,
compressedDepth,
stereoCameraModel,
@@ -4569,7 +4587,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
0,
compressedUserData):
SensorData(
LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
laserScan.angleMin(),
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
compressedImage,
compressedDepth,
cameraModels,
@@ -4619,7 +4650,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
SensorData(
LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
laserScan.angleMin(),
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
@@ -4627,7 +4671,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
0,
compressedUserData):
SensorData(
LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform()):
LaserScan(compressedScan,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
laserScan.angleMin(),
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform()),
cv::Mat(),
cv::Mat(),
cameraModels,
@@ -4648,6 +4705,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
s->sensorData().setGroundTruth(data.groundTruth());
s->sensorData().setGPS(data.gps());
s->sensorData().setEnvSensors(data.envSensors());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

8
corelib/src/OccupancyGrid.cpp
View File

@@ -305,13 +305,13 @@ void OccupancyGrid::createLocalMap(
}
float maxRange = cloudMaxDepth_;
if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
{
maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().rangeMax()?cloudMaxDepth_:node.sensorData().laserScanRaw().rangeMax();
}
else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
{
maxRange = node.sensorData().laserScanRaw().maxRange();
maxRange = node.sensorData().laserScanRaw().rangeMax();
}
util3d::occupancy2DFromLaserScan(
util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),

22
corelib/src/RegistrationIcp.cpp
View File

@@ -610,7 +610,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
// Load point clouds
DP data = laserScanToDP(fromScan);
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess * toScan.localTransform()));
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess * toScan.localTransform()));
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -786,7 +786,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
fromScan.rangeMax(),
LaserScan::kXYNormal,
fromScan.localTransform()));
}
@@ -796,7 +796,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
fromScan.rangeMax(),
LaserScan::kXYZNormal,
fromScan.localTransform()));
}
@@ -806,7 +806,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
toScan.rangeMax(),
LaserScan::kXYNormal,
toScan.localTransform()));
}
@@ -816,7 +816,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
toScan.rangeMax(),
LaserScan::kXYZNormal,
toScan.localTransform()));
}
@@ -833,7 +833,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
// Load point clouds
DP data = laserScanToDP(fromScan);
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -905,7 +905,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
fromScan.rangeMax(),
LaserScan::kXY,
fromScan.localTransform()));
}
@@ -915,7 +915,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
fromScan.rangeMax(),
LaserScan::kXYZ,
fromScan.localTransform()));
}
@@ -925,7 +925,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
toScan.rangeMax(),
LaserScan::kXY,
toScan.localTransform()));
}
@@ -935,7 +935,7 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan(
util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
toScan.rangeMax(),
LaserScan::kXYZ,
toScan.localTransform()));
}
@@ -948,7 +948,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
// Load point clouds
DP data = laserScanToDP(fromScan);
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
// Compute the transformation to express data in ref
PM::TransformationParameters T;

15
corelib/src/Rtabmap.cpp
View File

@@ -824,7 +824,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
double stamp = 0.0;
std::vector<float> v;
GPS gps;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true);
EnvSensors sensors;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
}
@@ -2928,7 +2929,8 @@ bool Rtabmap::process(
Transform groundTruth;
std::vector<float> velocity;
GPS gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false);
EnvSensors sensors;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -2942,6 +2944,7 @@ bool Rtabmap::process(
signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
signatures.at(iter->first).sensorData().setGPS(gps);
signatures.at(iter->first).sensorData().setEnvSensors(sensors);
if(_computeRMSE && !groundTruth.isNull())
{
groundTruths.insert(std::make_pair(iter->first, groundTruth));
@@ -3801,7 +3804,8 @@ void Rtabmap::get3DMap(
Transform groundTruth;
std::vector<float> velocity;
GPS gps;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true);
EnvSensors sensors;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
@@ -3825,6 +3829,7 @@ void Rtabmap::get3DMap(
signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
signatures.at(*iter).sensorData().setGPS(gps);
signatures.at(*iter).sensorData().setEnvSensors(sensors);
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3879,7 +3884,8 @@ void Rtabmap::getGraph(
Transform groundTruth;
std::vector<float> velocity;
GPS gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global);
EnvSensors sensors;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -3908,6 +3914,7 @@ void Rtabmap::getGraph(
signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
signatures->at(iter->first).sensorData().setGPS(gps);
signatures->at(iter->first).sensorData().setEnvSensors(sensors);
}
}
}

19
corelib/src/SensorData.cpp
View File

@@ -632,7 +632,14 @@ void SensorData::uncompressData(
_laserScanRaw = *laserScanRaw;
if(_laserScanCompressed.format() == LaserScan::kUnknown)
{
_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanRaw.format(), _laserScanCompressed.localTransform());
if(_laserScanCompressed.angleIncrement() > 0.0f)
{
_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanRaw.format(), _laserScanCompressed.rangeMin(), _laserScanCompressed.rangeMax(), _laserScanCompressed.angleMin(), _laserScanCompressed.angleMax(), _laserScanCompressed.angleIncrement(), _laserScanCompressed.localTransform());
}
else
{
_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanCompressed.maxPoints(), _laserScanCompressed.rangeMax(), _laserScanRaw.format(), _laserScanCompressed.localTransform());
}
}
}
if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
@@ -780,8 +787,14 @@ void SensorData::uncompressDataConst(
}
if(laserScanRaw && laserScanRaw->isEmpty())
{
*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanCompressed.format(), _laserScanCompressed.localTransform());
if(_laserScanCompressed.angleIncrement() > 0.0f)
{
*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.format(), _laserScanCompressed.rangeMin(), _laserScanCompressed.rangeMax(), _laserScanCompressed.angleMin(), _laserScanCompressed.angleMax(), _laserScanCompressed.angleIncrement(), _laserScanCompressed.localTransform());
}
else
{
*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.maxPoints(), _laserScanCompressed.rangeMax(), _laserScanCompressed.format(), _laserScanCompressed.localTransform());
}
if(laserScanRaw->isEmpty())
{
if(_laserScanCompressed.isEmpty())

136
corelib/src/StereoCameraModel.cpp
View File

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
@@ -358,6 +359,141 @@ bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
return false;
}
std::vector<unsigned char> StereoCameraModel::serialize() const
{
std::vector<unsigned char> leftData = left_.serialize();
std::vector<unsigned char> rightData = right_.serialize();
const int headerSize = 10;
int header[headerSize] = {
RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
1, //stereo // 3
(int)R_.total(), (int)T_.total(), (int)E_.total(), (int)F_.total(), // 4,5,6,7
(int)leftData.size(), (int)rightData.size()}; // 8,9
UDEBUG("Header: %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]);
std::vector<unsigned char> data(
sizeof(int)*headerSize +
sizeof(double)*(R_.total()+T_.total()+E_.total()+F_.total()) +
leftData.size() + rightData.size());
memcpy(data.data(), header, sizeof(int)*headerSize);
int index = sizeof(int)*headerSize;
if(!R_.empty())
{
memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
index+=sizeof(double)*(R_.total());
}
if(!T_.empty())
{
memcpy(data.data()+index, T_.data, sizeof(double)*(T_.total()));
index+=sizeof(double)*(T_.total());
}
if(!E_.empty())
{
memcpy(data.data()+index, E_.data, sizeof(double)*(E_.total()));
index+=sizeof(double)*(E_.total());
}
if(!F_.empty())
{
memcpy(data.data()+index, F_.data, sizeof(double)*(F_.total()));
index+=sizeof(double)*(F_.total());
}
if(leftData.size())
{
memcpy(data.data()+index, leftData.data(), leftData.size());
index+=leftData.size();
}
if(rightData.size())
{
memcpy(data.data()+index, rightData.data(), rightData.size());
index+=rightData.size();
}
return data;
}
unsigned int StereoCameraModel::deserialize(const std::vector<unsigned char>& data)
{
return deserialize(data.data(), data.size());
}
unsigned int StereoCameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
{
*this = StereoCameraModel();
int headerSize = 10;
if(dataSize >= sizeof(int)*headerSize)
{
int iR = 4;
int iT = 5;
int iE = 6;
int iF = 7;
int iLeft = 8;
int iRight = 9;
const int * header = (const int *)data;
UDEBUG("Header: %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]);
int type = header[3];
if(type==1)
{
unsigned int requiredDataSize = sizeof(int)*headerSize +
sizeof(double)*(header[iR]+header[iT]+header[iE]+header[iF]) +
header[iLeft] + header[iRight];
UASSERT_MSG(dataSize >= requiredDataSize,
uFormat("dataSize=%d != required=%d (header: version %d.%d.%d type=%d R=%d T=%d E=%d F=%d Left=%d Right=%d)",
dataSize,
requiredDataSize,
header[0], header[1], header[2], header[3],
header[iR], header[iT], header[iE],header[iF], header[iLeft], header[iRight]).c_str());
unsigned int index = sizeof(int)*headerSize;
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[iT] != 0)
{
UASSERT(header[iT] == 3);
T_ = cv::Mat(3, 1, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(T_.total());
}
if(header[iE] != 0)
{
UASSERT(header[iE] == 9);
E_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(E_.total());
}
if(header[iF] != 0)
{
UASSERT(header[iF] == 9);
F_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
index+=sizeof(double)*(F_.total());
}
if(header[iLeft] != 0)
{
index += left_.deserialize((data+index), header[iLeft]);
}
if(header[iRight] != 0)
{
index += right_.deserialize((data+index), header[iRight]);
}
UASSERT(index <= dataSize);
return index;
}
else
{
UERROR("Serialized calibration is not stereo (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;
}
void StereoCameraModel::scale(double scale)
{
left_ = left_.scaled(scale);

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

@@ -23,7 +23,7 @@ CREATE TABLE Node (
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
env_sensors BLOB, -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
time_enter DATE,
PRIMARY KEY (id)
);
@@ -87,6 +87,15 @@ CREATE TABLE Feature (
FOREIGN KEY (node_id) REFERENCES Node(id)
);
--
CREATE TABLE Tag (
node_id INTEGER NOT NULL,
tag_id INTEGER NOT NULL,
stamp FLOAT NOT NULL,
transform BLOB NOT NULL, -- 3x4 float, /base_link -> /tag_frame
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE Info (
STM_size INTEGER,
last_sign_added INTEGER,

70
corelib/src/util3d_filtering.cpp
View File

@@ -130,7 +130,27 @@ LaserScan commonFiltering(
}
int previousSize = scan.size();
int scanMaxPtsTmp = scan.maxPoints();
scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
if(scan.angleIncrement() > 0.0f)
{
scan = LaserScan(
cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
scan.format(),
rangeMin>0.0f&&rangeMin>scan.rangeMin()?rangeMin:scan.rangeMin(),
rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
scan.angleMin(),
scan.angleMax(),
scan.angleIncrement() * (float)downsamplingStep,
scan.localTransform());
}
else
{
scan = LaserScan(
cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
scanMaxPtsTmp/downsamplingStep,
rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
scan.format(),
scan.localTransform());
}
UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
}
@@ -154,16 +174,16 @@ LaserScan commonFiltering(
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
else
{
if(scan.hasNormals())
{
UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
}
}
}
@@ -186,12 +206,19 @@ LaserScan commonFiltering(
if(scan.is2d())
{
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
else
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
@@ -203,11 +230,11 @@ LaserScan commonFiltering(
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYI, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
}
}
}
@@ -231,12 +258,19 @@ LaserScan commonFiltering(
if(scan.is2d())
{
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
else
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
@@ -248,11 +282,11 @@ LaserScan commonFiltering(
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXY, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXY, scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
}
}
}
@@ -307,7 +341,11 @@ LaserScan rangeFiltering(
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
}
}
@@ -334,7 +372,11 @@ LaserScan downsample(
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
return LaserScan(output, scan.maxPoints()/step, scan.maxRange(), scan.format(), scan.localTransform());
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
}
return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
}
}
template<typename PointT>

34
corelib/src/util3d_surface.cpp
View File

@@ -2148,7 +2148,7 @@ LaserScan computeNormals(
{
UASSERT(!laserScan.is2d());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
}
else if(laserScan.hasIntensity())
@@ -2159,12 +2159,20 @@ LaserScan computeNormals(
if(laserScan.is2d())
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
if(laserScan.angleIncrement() > 0.0f)
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
}
else
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYINormal, laserScan.localTransform());
}
}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZINormal, laserScan.localTransform());
}
}
}
@@ -2176,12 +2184,19 @@ LaserScan computeNormals(
if(laserScan.is2d())
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
if(laserScan.angleIncrement() > 0.0f)
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
}
else
{
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYNormal, laserScan.localTransform());
}
}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZNormal, laserScan.localTransform());
}
}
}
@@ -2854,7 +2869,14 @@ LaserScan adjustNormalsToViewPoint(
}
}
}
return LaserScan(output, scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
else
{
return LaserScan(output, scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
}
}
return scan;
}

2
corelib/src/util3d_transforms.cpp
View File

@@ -83,7 +83,7 @@ LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & tran
}
}
}
return LaserScan(output, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform());
return LaserScan(output, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(