i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

3
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -115,6 +115,9 @@ public:
bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory) const;
std::vector<unsigned char> serialize() const;
unsigned int deserialize(const std::vector<unsigned char>& data);
unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
CameraModel scaled(double scale) const;
CameraModel roi(const cv::Rect & roi) const;

6
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -165,8 +165,9 @@ public:
void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void loadTags(int signatureId, std::map<int, TransformStamped> & tags) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
@@ -259,11 +260,12 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

11
corelib/include/rtabmap/core/DBDriverSqlite3.h
View File

@@ -131,11 +131,12 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -151,18 +152,17 @@ private:
std::string queryStepSensorData() const;
std::string queryStepLinkUpdate() const;
std::string queryStepLink() const;
std::string queryStepTag() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepOccupancyGridUpdate() const;
void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
void stepImage(
sqlite3_stmt * ppStmt,
int id,
const cv::Mat & imageBytes) const;
void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepTag(sqlite3_stmt * ppStmt, int nodeId, int tagId, const TransformStamped & pose) 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 cv::Point3f & pt, const cv::Mat & descriptor) const;
void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -175,6 +175,7 @@ private:
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
void loadTagsQuery(std::list<Signature *> & signatures) const;
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
protected:

85
corelib/include/rtabmap/core/EnvSensor.h Normal file
View File

@@ -0,0 +1,85 @@
/*
Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
namespace rtabmap {
class EnvSensor
{
public:
enum Type {
// built-in types
kUndefined = 0,
kWifiSignalStrength, // dBm
kAmbientTemperature, // Celcius
kAmbientAirPressure, // hPa
kAmbientLight, // lx
kAmbientRelativeHumidity, // %
// user types
kCustomSensor1 = 100,
kCustomSensor2,
kCustomSensor3,
kCustomSensor4,
kCustomSensor5,
kCustomSensor6,
kCustomSensor7,
kCustomSensor8,
kCustomSensor9
};
public:
EnvSensor() :
type_(kUndefined),
value_(0.0),
stamp_(0.0)
{}
EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
type_(type),
value_(value),
stamp_(stamp)
{}
virtual ~EnvSensor() {}
const Type & type() const {return type_;}
const double & value() const {return value_;}
const double & stamp() const {return stamp_;}
private:
Type type_;
double value_;
double stamp_;
};
typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

78
corelib/include/rtabmap/core/GPS.h Normal file
View File

@@ -0,0 +1,78 @@
/*
Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
#include <rtabmap/core/GeodeticCoords.h>
namespace rtabmap {
class GPS
{
public:
GPS():
stamp_(0.0),
longitude_(0.0),
latitude_(0.0),
altitude_(0.0),
error_(0.0),
bearing_(0.0)
{}
GPS(const double & stamp,
const double & longitude,
const double & latitude,
const double & altitude,
const double & error,
const double & bearing):
stamp_(stamp),
longitude_(longitude),
latitude_(latitude),
altitude_(altitude),
error_(error),
bearing_(bearing)
{}
const double & stamp() const {return stamp_;}
const double & longitude() const {return longitude_;}
const double & latitude() const {return latitude_;}
const double & altitude() const {return altitude_;}
const double & error() const {return error_;}
const double & bearing() const {return bearing_;}
GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
private:
double stamp_; // in sec
double longitude_; // DD
double latitude_; // DD
double altitude_; // m
double error_; // m
double bearing_; // deg (North 0->360 clockwise)
};
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

41
corelib/include/rtabmap/core/GeodeticCoords.h
View File

@@ -81,47 +81,6 @@ private:
double altitude_; // m
};
class GPS
{
public:
GPS():
stamp_(0.0),
longitude_(0.0),
latitude_(0.0),
altitude_(0.0),
error_(0.0),
bearing_(0.0)
{}
GPS(const double & stamp,
const double & longitude,
const double & latitude,
const double & altitude,
const double & error,
const double & bearing):
stamp_(stamp),
longitude_(longitude),
latitude_(latitude),
altitude_(altitude),
error_(error),
bearing_(bearing)
{}
const double & stamp() const {return stamp_;}
const double & longitude() const {return longitude_;}
const double & latitude() const {return latitude_;}
const double & altitude() const {return altitude_;}
const double & error() const {return error_;}
const double & bearing() const {return bearing_;}
GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
private:
double stamp_; // in sec
double longitude_; // DD
double latitude_; // DD
double altitude_; // m
double error_; // m
double bearing_; // deg (North 0->360 clockwise)
};
}
#endif /* GEODETICCOORDS_H_ */

2
corelib/include/rtabmap/core/Graph.h
View File

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <list>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <rtabmap/core/GPS.h>
namespace rtabmap {
class Memory;

46
corelib/include/rtabmap/core/LaserScan.h
View File

@@ -54,16 +54,44 @@ public:
static bool isScanHasNormals(const Format & format);
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints = 0,
int maxRange = 0,
const Transform & localTransform = Transform::getIdentity());
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleInc,
const Transform & localTransform = Transform::getIdentity());
public:
LaserScan();
LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
LaserScan(const cv::Mat & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform = Transform::getIdentity());
LaserScan(const cv::Mat & data,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform = Transform::getIdentity());
const cv::Mat & data() const {return data_;}
int maxPoints() const {return maxPoints_;}
float maxRange() const {return maxRange_;}
Format format() const {return format_;}
int maxPoints() const {return maxPoints_;}
float rangeMin() const {return rangeMin_;}
float rangeMax() const {return rangeMax_;}
float angleMin() const {return angleMin_;}
float angleMax() const {return angleMax_;}
float angleIncrement() const {return angleIncrement_;}
Transform localTransform() const {return localTransform_;}
bool isEmpty() const {return data_.empty();}
@@ -74,7 +102,7 @@ public:
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
LaserScan clone() const;
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
@@ -84,9 +112,13 @@ public:
private:
cv::Mat data_;
int maxPoints_;
float maxRange_;
Format format_;
int maxPoints_;
float rangeMin_;
float rangeMax_;
float angleMin_;
float angleMax_;
float angleIncrement_;
Transform localTransform_;
};

1
corelib/include/rtabmap/core/Memory.h
View File

@@ -186,6 +186,7 @@ public:
Transform & groundTruth,
std::vector<float> & velocity,
GPS & gps,
EnvSensors & sensors,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int nodeId, bool uncompressedData = false) const;

23
corelib/include/rtabmap/core/SensorData.h
View File

@@ -33,11 +33,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/LaserScan.h>
#include <rtabmap/core/IMU.h>
#include <rtabmap/core/GPS.h>
#include <rtabmap/core/EnvSensor.h>
namespace rtabmap
{
@@ -235,18 +236,16 @@ public:
const Transform & globalPose() const {return globalPose_;}
const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
void setGPS(const GPS & gps)
{
gps_ = gps;
}
void setGPS(const GPS & gps) {gps_ = gps;}
const GPS & gps() const {return gps_;}
void setIMU(const IMU & imu)
{
imu_ = imu;
}
void setIMU(const IMU & imu) {imu_ = imu; }
const IMU & imu() const {return imu_;}
void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;}
void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
const EnvSensors & envSensors() const {return _envSensors;}
long getMemoryUsed() const; // Return memory usage in Bytes
void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
@@ -281,6 +280,12 @@ private:
float _cellSize;
cv::Point3f _viewPoint;
// environmental sensors
EnvSensors _envSensors;
// tags
std::map<int, TransformStamped> _tags;
// features
std::vector<cv::KeyPoint> _keypoints;
std::vector<cv::Point3f> _keypoints3D;

5
corelib/include/rtabmap/core/Signature.h
View File

@@ -90,6 +90,10 @@ public:
void removeLink(int idTo);
void removeVirtualLinks();
void setTags(const std::map<int, TransformStamped> & tags) {_tags = tags;}
void addTag(int id, const TransformStamped & pose) {_tags.insert(std::make_pair(id, pose));}
const std::map<int, TransformStamped> & getTags() const {return _tags;}
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -141,6 +145,7 @@ private:
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
std::map<int, TransformStamped> _tags;
int _weight;
std::string _label;
bool _saved; // If it's saved to bd

3
corelib/include/rtabmap/core/StereoCameraModel.h
View File

@@ -97,6 +97,9 @@ public:
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
bool saveStereoTransform(const std::string & directory) const;
std::vector<unsigned char> serialize() const;
unsigned int deserialize(const std::vector<unsigned char>& data);
unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}

15
corelib/include/rtabmap/core/Transform.h
View File

@@ -155,6 +155,21 @@ private:
RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
class TransformStamped
{
public:
TransformStamped(const Transform & transform, const double & stamp) :
transform_(transform),
stamp_(stamp)
{}
const Transform & transform() const {return transform_;}
const double & stamp() const {return stamp_;}
private:
Transform transform_;
double stamp_;
};
}
#endif /* TRANSFORM_H_ */

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(