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

Integration of CLAMS depth calibration #114

Browse Source
This commit is contained in:
matlabbe
2016-09-19 14:01:06 -04:00
parent 861cc437f4
commit 3434b95d68
30 changed files with 2867 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraInfo.h
+4
View File
@@ -45,6 +45,8 @@ public:
timeMirroring(0.0f),
timeImageDecimation(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
@@ -58,6 +60,8 @@ public:
float timeMirroring;
float timeImageDecimation;
float timeScanFromDepth;
float timeUndistortDepth;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
};
corelib/include/rtabmap/core/CameraModel.h
+9 -1
View File
@@ -76,7 +76,8 @@ public:
void initRectificationMap();
bool isValidForProjection() const {return fx()>0.0 && fy()>0.0;}
bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
bool isValidForRectification() const
{
return imageSize_.width>0 &&
@@ -124,6 +125,13 @@ public:
cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
cv::Mat rectifyDepth(const cv::Mat & raw) const;
// Project 2D pixel to 3D (in /camera_link frame)
void project(float u, float v, float depth, float & x, float & y, float & z) const;
// Reproject 3D point (in /camera_link frame) to pixel
void reproject(float x, float y, float z, float & u, float & v) const;
void reproject(float x, float y, float z, int & u, int & v) const;
bool inFrame(int u, int v) const;
private:
std::string name_;
cv::Size imageSize_;
corelib/include/rtabmap/core/CameraThread.h
+8 -1
View File
@@ -33,6 +33,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
@@ -56,6 +61,8 @@ public:
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setImageRate(float imageRate);
void setDistortionModel(const std::string & path);
void setScanFromDepth(
bool enabled,
@@ -74,7 +81,6 @@ public:
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
void setImageRate(float imageRate);
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
@@ -95,6 +101,7 @@ private:
float _scanVoxelSize;
int _scanNormalsK;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
};
} // namespace rtabmap
corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h
+130
View File
@@ -0,0 +1,130 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef DISCRETE_DEPTH_DISTORTION_MODEL_H
#define DISCRETE_DEPTH_DISTORTION_MODEL_H
#include <assert.h>
#include <vector>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <rtabmap/utilite/UMutex.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
namespace clams
{
class RTABMAP_EXP DiscreteFrustum
{
public:
DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0);
//! z value, not distance to origin.
//! thread-safe.
void addExample(double ground_truth, double measurement);
int index(double z) const;
void undistort(double* z) const;
void interpolatedUndistort(double* z) const;
void serialize(std::ostream& out) const;
void deserialize(std::istream& in);
protected:
double max_dist_;
int num_bins_;
double bin_depth_;
Eigen::VectorXf counts_;
Eigen::VectorXf total_numerators_;
Eigen::VectorXf total_denominators_;
Eigen::VectorXf multipliers_;
friend class DiscreteDepthDistortionModel;
};
class RTABMAP_EXP DiscreteDepthDistortionModel
{
public:
DiscreteDepthDistortionModel() :
width_(0),
height_(0),
bin_width_(0),
bin_height_(0),
bin_depth_(0),
num_bins_x_(0),
num_bins_y_(0),
training_samples_(0)
{}
virtual ~DiscreteDepthDistortionModel();
DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1);
DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other);
DiscreteDepthDistortionModel& operator=(const DiscreteDepthDistortionModel& other);
void undistort(cv::Mat & depth) const;
//! Returns the number of training examples it used from this pair.
//! Thread-safe.
size_t accumulate(const cv::Mat& ground_truth, const cv::Mat& measurement);
void addExample(int v, int u, double ground_truth, double measurement);
void save(const std::string& path) const;
void load(const std::string& path);
void serialize(std::ostream& out) const;
void deserialize(std::istream& in);
cv::Mat visualize(const std::string& path = "") const;
int getWidth() const {return width_;}
int getHeight() const {return height_;}
size_t getTrainingSamples() const {return training_samples_;}
bool isValid() const
{
return !frustums_.empty();
}
protected:
//! Image width.
int width_;
//! Image height.
int height_;
//! Width of each bin in pixels.
int bin_width_;
//! Height of each bin in pixels.
int bin_height_;
//! Depth of each bin in meters.
double bin_depth_;
int num_bins_x_;
int num_bins_y_;
//! frustums_[y][x]
std::vector< std::vector<DiscreteFrustum*> > frustums_;
size_t training_samples_;
void deleteFrustums();
DiscreteFrustum& frustum(int y, int x);
const DiscreteFrustum& frustum(int y, int x) const;
UMutex mutex_;
};
} // namespace clams
#endif // DISCRETE_DEPTH_DISTORTION_MODEL_H
corelib/include/rtabmap/core/clams/frame_projector.h
+96
View File
@@ -0,0 +1,96 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef FRAME_PROJECTOR_H
#define FRAME_PROJECTOR_H
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/CameraModel.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#define MAX_MULT 1.3
#define MIN_MULT 0.7
namespace clams
{
//! "Projective" point comes from the OpenNI terminology, and refers to (u, v, z), i.e.
//! pixel id and depth value. Here I've added color, too, so that this represents everything
//! that is known about a pixel in an RBGD camera.
class ProjectivePoint
{
public:
ProjectivePoint() :
u_(0),
v_(0),
z_(0.0f) {}
int u_;
int v_;
float z_; // in meters
};
//! This is essentially a pinhole camera model for an RGBD sensor, with
//! some extra functions added on for use during calibration.
class RTABMAP_EXP FrameProjector
{
public:
// For storing z values in meters. This is not Euclidean distance.
typedef std::vector< std::vector< std::vector<double> > > RangeIndex;
FrameProjector(const rtabmap::CameraModel & model);
RangeIndex cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const;
//! transform is applied to the map, then projected into a depth index.
//! The best depth estimate from the map corresponding to the measurement depth frame
//! will be returned.
cv::Mat estimateMapDepth(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
const rtabmap::Transform & transform,
const cv::Mat & measurement,
double coneRadius = 0.02,
double coneStdevThresh = 0.03) const;
pcl::PointXYZ project(const ProjectivePoint& ppt) const;
ProjectivePoint reproject(const pcl::PointXYZ& pt) const;
protected:
bool coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
int uc, int vc, double radius, double measurement_depth,
double* mean, double* stdev) const;
private:
rtabmap::CameraModel model_;
};
} // namespace clams
#endif // FRAME_PROJECTOR_H
corelib/include/rtabmap/core/clams/slam_calibrator.h
+52
View File
@@ -0,0 +1,52 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef SLAM_CALIBRATOR_H
#define SLAM_CALIBRATOR_H
#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
#include <rtabmap/core/SensorData.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
namespace clams
{
DiscreteDepthDistortionModel RTABMAP_EXP calibrate(
const std::map<int, rtabmap::SensorData> & sequence,
const std::map<int, rtabmap::Transform> & trajectory,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
double coneRadius = 0.02,
double coneStdevThresh = 0.03);
}
#endif // SLAM_CALIBRATOR_H
corelib/src/CMakeLists.txt
+6
View File
@@ -74,6 +74,12 @@ SET(SRC_FILES
FlannIndex.cpp
sqlite3/sqlite3.c
#clams stuff
clams/discrete_depth_distortion_model_helpers.cpp
clams/discrete_depth_distortion_model.cpp
clams/frame_projector.cpp
clams/slam_calibrator.cpp
)
IF(OpenCV_VERSION_MAJOR EQUAL 2)
corelib/src/CameraModel.cpp
+61 -4
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
@@ -75,9 +76,19 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
{
cy = double(imageSize.height)/2.0-0.5;
}
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -110,9 +121,19 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
{
cy = double(imageSize.height)/2.0-0.5;
}
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -501,4 +522,40 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
}
}
// resulting 3D point is in /camera_link frame
void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
{
if(depth > 0.0f)
{
// Fill in XYZ
x = (u - cx()) * depth / fx();
y = (v - cy()) * depth / fy();
z = depth;
}
else
{
x = y = z = std::numeric_limits<float>::quiet_NaN();
}
}
// 3D point is in /camera_link frame
void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
{
UASSERT(z!=0.0f);
float invZ = 1.0f/z;
u = (fx()*x)*invZ + cx();
v = (fy()*y)*invZ + cy();
}
void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
{
UASSERT(z!=0.0f);
float invZ = 1.0f/z;
u = (fx()*x)*invZ + cx();
v = (fy()*y)*invZ + cy();
}
bool CameraModel::inFrame(int u, int v) const
{
return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
}
} /* namespace rtabmap */
corelib/src/CameraThread.cpp
+49 -1
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -56,7 +57,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_scanMinDepth(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_stereoDense(new StereoBM(parameters))
_stereoDense(new StereoBM(parameters)),
_distortionModel(0)
{
UASSERT(_camera != 0);
}
@@ -69,6 +71,10 @@ CameraThread::~CameraThread()
{
delete _camera;
}
if(_distortionModel)
{
delete _distortionModel;
}
delete _stereoDense;
}
@@ -80,8 +86,29 @@ void CameraThread::setImageRate(float imageRate)
}
}
void CameraThread::setDistortionModel(const std::string & path)
{
if(_distortionModel)
{
delete _distortionModel;
_distortionModel = 0;
}
if(!path.empty())
{
_distortionModel = new clams::DiscreteDepthDistortionModel();
_distortionModel->load(path);
if(!_distortionModel->isValid())
{
UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str());
delete _distortionModel;
_distortionModel = 0;
}
}
}
void CameraThread::mainLoop()
{
UTimer totalTime;
UDEBUG("");
CameraInfo info;
SensorData data = _camera->takeImage(&info);
@@ -92,6 +119,26 @@ void CameraThread::mainLoop()
{
data.setDepthOrRightRaw(cv::Mat());
}
if(_distortionModel && !data.depthRaw().empty())
{
UTimer timer;
if(_distortionModel->getWidth() == data.depthRaw().cols &&
_distortionModel->getHeight() == data.depthRaw().rows )
{
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
_distortionModel->undistort(depth);
data.setDepthOrRightRaw(depth);
}
else
{
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
_distortionModel->getWidth(), _distortionModel->getHeight(),
data.depthRaw().cols, data.depthRaw().rows);
}
info.timeUndistortDepth = timer.ticks();
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");
@@ -228,6 +275,7 @@ void CameraThread::mainLoop()
}
info.cameraName = _camera->getSerial();
info.timeTotal = totalTime.ticks();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())
corelib/src/clams/discrete_depth_distortion_model.cpp
+344
View File
@@ -0,0 +1,344 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include "eigen_extensions/eigen_extensions.h"
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteFrustum::DiscreteFrustum(int smoothing, double bin_depth) :
max_dist_(10),
bin_depth_(bin_depth)
{
num_bins_ = ceil(max_dist_ / bin_depth_);
counts_ = VectorXf::Ones(num_bins_) * smoothing;
total_numerators_ = VectorXf::Ones(num_bins_) * smoothing;
total_denominators_ = VectorXf::Ones(num_bins_) * smoothing;
multipliers_ = VectorXf::Ones(num_bins_);
}
void DiscreteFrustum::addExample(double ground_truth, double measurement)
{
double mult = ground_truth / measurement;
if(mult > MAX_MULT || mult < MIN_MULT)
return;
int idx = min(num_bins_ - 1, (int)floor(measurement / bin_depth_));
UASSERT(idx >= 0);
total_numerators_(idx) += ground_truth * ground_truth;
total_denominators_(idx) += ground_truth * measurement;
++counts_(idx);
multipliers_(idx) = total_numerators_(idx) / total_denominators_(idx);
}
inline int DiscreteFrustum::index(double z) const
{
return min(num_bins_ - 1, (int)floor(z / bin_depth_));
}
inline void DiscreteFrustum::undistort(double* z) const
{
*z *= multipliers_.coeffRef(index(*z));
}
void DiscreteFrustum::interpolatedUndistort(double* z) const
{
int idx = index(*z);
double start = bin_depth_ * idx;
int idx1;
if(*z - start < bin_depth_ / 2)
idx1 = idx;
else
idx1 = idx + 1;
int idx0 = idx1 - 1;
if(idx0 < 0 || idx1 >= num_bins_ || counts_(idx0) < 50 || counts_(idx1) < 50) {
undistort(z);
return;
}
double z0 = (idx0 + 1) * bin_depth_ - bin_depth_ * 0.5;
double coeff1 = (*z - z0) / bin_depth_;
double coeff0 = 1.0 - coeff1;
double mult = coeff0 * multipliers_.coeffRef(idx0) + coeff1 * multipliers_.coeffRef(idx1);
*z *= mult;
}
void DiscreteFrustum::serialize(std::ostream& out) const
{
eigen_extensions::serializeScalar(max_dist_, out);
eigen_extensions::serializeScalar(num_bins_, out);
eigen_extensions::serializeScalar(bin_depth_, out);
eigen_extensions::serialize(counts_, out);
eigen_extensions::serialize(total_numerators_, out);
eigen_extensions::serialize(total_denominators_, out);
eigen_extensions::serialize(multipliers_, out);
}
void DiscreteFrustum::deserialize(std::istream& in)
{
eigen_extensions::deserializeScalar(in, &max_dist_);
eigen_extensions::deserializeScalar(in, &num_bins_);
eigen_extensions::deserializeScalar(in, &bin_depth_);
eigen_extensions::deserialize(in, &counts_);
eigen_extensions::deserialize(in, &total_numerators_);
eigen_extensions::deserialize(in, &total_denominators_);
eigen_extensions::deserialize(in, &multipliers_);
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other)
{
*this = other;
}
DiscreteDepthDistortionModel& DiscreteDepthDistortionModel::operator=(const DiscreteDepthDistortionModel& other)
{
width_ = other.width_;
height_ = other.height_;
bin_width_ = other.bin_width_;
bin_height_ = other.bin_height_;
bin_depth_ = other.bin_depth_;
num_bins_x_ = other.num_bins_x_;
num_bins_y_ = other.num_bins_y_;
training_samples_ = other.training_samples_;
frustums_ = other.frustums_;
for(size_t i = 0; i < frustums_.size(); ++i)
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(*other.frustums_[i][j]);
return *this;
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(int width, int height,
int bin_width, int bin_height,
double bin_depth,
int smoothing) :
width_(width),
height_(height),
bin_width_(bin_width),
bin_height_(bin_height),
bin_depth_(bin_depth)
{
UASSERT(width_ % bin_width_ == 0);
UASSERT(height_ % bin_height_ == 0);
num_bins_x_ = width_ / bin_width_;
num_bins_y_ = height_ / bin_height_;
frustums_.resize(num_bins_y_);
for(size_t i = 0; i < frustums_.size(); ++i) {
frustums_[i].resize(num_bins_x_, NULL);
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(smoothing, bin_depth);
}
training_samples_ = 0;
}
void DiscreteDepthDistortionModel::deleteFrustums()
{
for(size_t y = 0; y < frustums_.size(); ++y)
for(size_t x = 0; x < frustums_[y].size(); ++x)
if(frustums_[y][x])
delete frustums_[y][x];
training_samples_ = 0;
}
DiscreteDepthDistortionModel::~DiscreteDepthDistortionModel()
{
deleteFrustums();
}
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
z = zf;
}
}
}
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
z = zf*1000;
}
}
}
}
void DiscreteDepthDistortionModel::addExample(int v, int u, double ground_truth, double measurement)
{
frustum(v, u).addExample(ground_truth, measurement);
}
size_t DiscreteDepthDistortionModel::accumulate(const cv::Mat& ground_truth,
const cv::Mat& measurement)
{
UASSERT(width_ == ground_truth.cols);
UASSERT(height_ == ground_truth.rows);
UASSERT(width_ == measurement.cols);
UASSERT(height_ == measurement.rows);
UASSERT(ground_truth.type() == CV_16UC1 || ground_truth.type() == CV_32FC1);
UASSERT(measurement.type() == CV_16UC1 || measurement.type() == CV_32FC1);
bool isGroundTruthInMM = ground_truth.type()==CV_16UC1;
bool isMeasurementInMM = measurement.type()==CV_16UC1;
size_t num_training_examples = 0;
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float gt = isGroundTruthInMM?float(ground_truth.at<unsigned short>(v,u))*0.001:ground_truth.at<float>(v,u);
if(gt == 0)
continue;
float meas = isMeasurementInMM?float(measurement.at<unsigned short>(v,u))*0.001:measurement.at<float>(v,u);
if(meas == 0)
continue;
UScopeMutex sm(mutex_);
frustum(v, u).addExample(gt, meas);
++num_training_examples;
}
}
training_samples_ += num_training_examples;
return num_training_examples;
}
void DiscreteDepthDistortionModel::load(const std::string& path)
{
ifstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
deserialize(f);
f.close();
}
void DiscreteDepthDistortionModel::save(const std::string& path) const
{
ofstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
serialize(f);
f.close();
}
void DiscreteDepthDistortionModel::serialize(std::ostream& out) const
{
out << "DiscreteDepthDistortionModel v01" << endl;
eigen_extensions::serializeScalar(width_, out);
eigen_extensions::serializeScalar(height_, out);
eigen_extensions::serializeScalar(bin_width_, out);
eigen_extensions::serializeScalar(bin_height_, out);
eigen_extensions::serializeScalar(bin_depth_, out);
eigen_extensions::serializeScalar(num_bins_x_, out);
eigen_extensions::serializeScalar(num_bins_y_, out);
eigen_extensions::serializeScalar(training_samples_, out);
for(int y = 0; y < num_bins_y_; ++y)
for(int x = 0; x < num_bins_x_; ++x)
frustums_[y][x]->serialize(out);
}
void DiscreteDepthDistortionModel::deserialize(std::istream& in)
{
string buf;
getline(in, buf);
assert(buf == "DiscreteDepthDistortionModel v01");
eigen_extensions::deserializeScalar(in, &width_);
eigen_extensions::deserializeScalar(in, &height_);
eigen_extensions::deserializeScalar(in, &bin_width_);
eigen_extensions::deserializeScalar(in, &bin_height_);
eigen_extensions::deserializeScalar(in, &bin_depth_);
eigen_extensions::deserializeScalar(in, &num_bins_x_);
eigen_extensions::deserializeScalar(in, &num_bins_y_);
eigen_extensions::deserializeScalar(in, &training_samples_);
deleteFrustums();
frustums_.resize(num_bins_y_);
for(size_t y = 0; y < frustums_.size(); ++y) {
frustums_[y].resize(num_bins_x_, NULL);
for(size_t x = 0; x < frustums_[y].size(); ++x) {
frustums_[y][x] = new DiscreteFrustum;
frustums_[y][x]->deserialize(in);
}
}
}
DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x)
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
const DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x) const
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
} // namespace clams
corelib/src/clams/discrete_depth_distortion_model_helpers.cpp
+207
View File
@@ -0,0 +1,207 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
using namespace std;
using namespace Eigen;
namespace clams
{
cv::Mat DiscreteDepthDistortionModel::visualize(const std::string& dir) const
{
bool writeFiles = !dir.empty() && UDirectory::exists(dir);
const DiscreteFrustum& reference_frustum = *frustums_[0][0];
int num_layers = reference_frustum.num_bins_;
// -- Set up for combined imagery.
int horiz_divider = 10;
int vert_divider = 20;
cv::Mat3b overview(cv::Size(width_ * 2 + horiz_divider, height_ * num_layers + vert_divider * (num_layers + 2)), cv::Vec3b(0, 0, 0));
vector<int> pub_layers;
for(int i = 0; i < num_layers; ++i)
pub_layers.push_back(i);
// pub_layers.push_back(1);
// pub_layers.push_back(2);
// pub_layers.push_back(3);
cv::Mat3b pub(cv::Size(width_, height_ * pub_layers.size() + vert_divider * (pub_layers.size() + 2)), cv::Vec3b(255, 255, 255));
for(int i = 0; i < num_layers; ++i) {
// -- Determine the path to save the image for this layer.
char buffer[50];
float mindepth = reference_frustum.bin_depth_ * i;
float maxdepth = reference_frustum.bin_depth_ * (i + 1);
sprintf(buffer, "%05.2f-%05.2f", mindepth, maxdepth);
ostringstream oss;
oss << dir << "/multipliers_" << buffer << ".png";
// -- Compute the multipliers visualization for this layer.
// Multiplier of 1 is black, >1 is red, <1 is blue. Think redshift.
cv::Mat3b mult(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < mult.rows; ++y) {
for(int x = 0; x < mult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1)
mult(y, x)[2] = min(255., 255 * (val - 1.0) / 0.25);
if(val < 1)
mult(y, x)[0] = min(255., 255 * (1.0 - val) / 0.25);
}
}
if(writeFiles)
{
cv::imwrite(oss.str(), mult);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Compute the counts visualization for this layer.
// 0 is black, 100 is white.
cv::Mat3b count(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < count.rows; ++y) {
for(int x = 0; x < count.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
uchar val = min(255., (double)(255 * frustum.counts_(i) / 100));
count(y, x)[0] = val;
count(y, x)[1] = val;
count(y, x)[2] = val;
}
}
oss.str("");
oss << dir << "/counts_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), count);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Make images showing the two, side-by-side.
cv::Mat3b combined(cv::Size(width_ * 2 + horiz_divider, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < combined.rows; ++y) {
for(int x = 0; x < combined.cols; ++x) {
if(x < count.cols)
combined(y, x) = count(y, x);
else if(x > count.cols + horiz_divider)
combined(y, x) = mult(y, x - count.cols - horiz_divider);
}
}
oss.str("");
oss << dir << "/combined_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), combined);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Append to the overview image.
for(int y = 0; y < combined.rows; ++y)
for(int x = 0; x < combined.cols; ++x)
overview(y + i * (combined.rows + vert_divider) + vert_divider, x) = combined(y, x);
// -- Compute the publication multipliers visualization for this layer.
// Multiplier of 1 is white, >1 is red, <1 is blue. Think redshift.
cv::Mat3b pubmult(cv::Size(width_, height_), cv::Vec3b(255, 255, 255));
for(int y = 0; y < pubmult.rows; ++y) {
for(int x = 0; x < pubmult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1) {
pubmult(y, x)[0] = 255 - min(255., 255 * (val - 1.0) / 0.1);
pubmult(y, x)[1] = 255 - min(255., 255 * (val - 1.0) / 0.1);
}
if(val < 1) {
pubmult(y, x)[1] = 255 - min(255., 255 * (1.0 - val) / 0.1);
pubmult(y, x)[2] = 255 - min(255., 255 * (1.0 - val) / 0.1);
}
}
}
// -- Append to publication image.
for(size_t j = 0; j < pub_layers.size(); ++j)
if(pub_layers[j] == i)
for(int y = 0; y < pubmult.rows; ++y)
for(int x = 0; x < pubmult.cols; ++x)
pub(y + j * (pubmult.rows + vert_divider) + vert_divider, x) = pubmult(y, x);
}
// -- Add a white bar at the top and bottom for reference.
for(int y = 0; y < overview.rows; ++y)
if(y < vert_divider || y > overview.rows - vert_divider)
for(int x = 0; x < overview.cols; ++x)
overview(y, x) = cv::Vec3b(255, 255, 255);
// -- Save overview image.
ostringstream oss;
oss << dir << "/overview.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save a small version for easy loading.
cv::Mat3b overview_scaled;
cv::resize(overview, overview_scaled, cv::Size(), 0.2, 0.2, cv::INTER_CUBIC);
oss.str("");
oss << dir << "/overview_scaled.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview_scaled);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save publication image.
oss.str("");
oss << dir << "/pub";
// for(size_t i = 0; i < pub_layers.size(); ++i)
// oss << "-" << setw(2) << setfill('0') << pub_layers[i];
oss << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), pub);
UINFO("Written \"%s\"", oss.str().c_str());
}
UASSERT(overview.rows == pub.rows);
cv::Mat3b targetImage(overview.rows, overview.cols/2 + pub.cols);
cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
pub.copyTo(roiB);
return targetImage;
}
} // namespace clams
corelib/src/clams/eigen_extensions/eigen_extensions.h
+312
View File
@@ -0,0 +1,312 @@
#ifndef EIGEN_EXTENSIONS_H
#define EIGEN_EXTENSIONS_H
#include <Eigen/Eigen>
//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>
//#include <boost/filesystem.hpp>
#include <stdint.h>
#include <fstream>
#include <iostream>
//#include <gzstream/gzstream.h>
namespace eigen_extensions {
inline void stdToEig(const std::vector<double>& std, Eigen::VectorXd* eig)
{
eig->resize(std.size());
for(size_t i = 0; i < std.size(); ++i)
eig->coeffRef(i) = std[i];
}
inline double stdev(const Eigen::VectorXd& vec)
{
double mean = vec.sum() / (double)vec.rows();
double total = 0;
for(int i = 0; i < vec.rows(); ++i)
total += (vec.coeffRef(i) - mean) * (vec.coeffRef(i) - mean);
double var = total / (double)vec.rows();
return sqrt(var);
}
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, const std::string& filename);
template<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
// -- SparseMatrix serialization.
template<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, std::ostream& strm);
template<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
// -- Scalar serialization
// TODO: Can you name these {de,}serialize() and still have the right
// functions get called when serializing matrices?
template<class T>
void serializeScalar(T val, std::ostream& strm);
template<class T>
void deserializeScalar(std::istream& strm, T* val);
/************************************************************
* Template implementations
************************************************************/
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int bytes = sizeof(S);
int rows = mat.rows();
int cols = mat.cols();
strm.write((char*)&bytes, sizeof(int));
strm.write((char*)&rows, sizeof(int));
strm.write((char*)&cols, sizeof(int));
strm.write((const char*)mat.data(), sizeof(S) * rows * cols);
}
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
int bytes;
int rows;
int cols;
strm.read((char*)&bytes, sizeof(int));
strm.read((char*)&rows, sizeof(int));
strm.read((char*)&cols, sizeof(int));
assert(bytes == sizeof(S));
S *buf = (S*) malloc(sizeof(S) * rows * cols);
strm.read((char*)buf, sizeof(S) * rows * cols);
*mat = Eigen::Map< Eigen::Matrix<S, T, U> >(buf, rows, cols);
free(buf);
}
/*
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& mat, const std::string& filename)
{
assert(filename.size() > 3);
if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) {
ogzstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
else {
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ofstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
}
template<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* mat)
{
assert(filename.size() > 3);
if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) {
igzstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
else {
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ifstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
}
*/
template<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, std::ostream& strm)
{
int bytes = sizeof(ScalarType);
int type = Options;
int outer = mat.outerSize();
int inner = mat.innerSize();
int nnz = mat.nonZeros();
strm.write((char*)&bytes, sizeof(int));
strm.write((char*)&type, sizeof(int));
strm.write((char*)&outer, sizeof(int));
strm.write((char*)&inner, sizeof(int));
strm.write((char*)&nnz, sizeof(int));
typedef typename Eigen::SparseMatrix<ScalarType, Options, IndexType>::InnerIterator InnerIterator;
for(IndexType i = 0; i < mat.outerSize(); ++i) {
int num = 0;
for(InnerIterator it(mat, i); it; ++it)
++num;
strm.write((const char*)&num, sizeof(num));
for(InnerIterator it(mat, i); it; ++it) {
int idx = it.index();
ScalarType buf = it.value();
strm.write((const char*)&idx, sizeof(idx));
strm.write((const char*)&buf, sizeof(buf));
}
}
}
template<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat)
{
int bytes;
int options;
int outer;
int inner;
int nnz;
strm.read((char*)&bytes, sizeof(int));
strm.read((char*)&options, sizeof(int));
strm.read((char*)&outer, sizeof(int));
strm.read((char*)&inner, sizeof(int));
strm.read((char*)&nnz, sizeof(int));
assert(bytes == sizeof(ScalarType));
assert(options == Options);
if(mat->IsRowMajor)
mat->resize(outer, inner);
else
mat->resize(inner, outer);
mat->reserve(nnz);
ScalarType buf;
for(int i = 0; i < mat->outerSize(); ++i) {
mat->startVec(i);
int num;
strm.read((char*)&num, sizeof(int));
int idx;
for(int j = 0; j < num; ++j) {
strm.read((char*)&idx, sizeof(idx));
strm.read((char*)&buf, sizeof(buf));
mat->insertBackByOuterInner(i, idx) = buf;
}
}
mat->finalize();
}
/*
template<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, const std::string& filename)
{
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ofstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
template<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat)
{
assert(filename.size() > 3);
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ifstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
*/
template<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int old_precision = strm.precision();
strm.precision(16);
strm << "% " << mat.rows() << " " << mat.cols() << std::endl;
strm << mat << std::endl;
strm.precision(old_precision);
}
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
// -- Read the header.
std::string line;
while(line.length() == 0) getline(strm, line);
assert(line[0] == '%');
std::istringstream iss(line.substr(1));
int rows;
int cols;
iss >> rows;
iss >> cols;
// -- Read in the data.
*mat = Eigen::Matrix<S, T, U>(rows, cols);
for(int y = 0; y < rows; ++y) {
getline(strm, line);
std::istringstream iss(line);
for(int x = 0; x < cols; ++x) {
iss >> mat->coeffRef(y, x);
}
}
}
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& mat, const std::string& filename)
{
assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0);
std::ofstream file;
file.open(filename.c_str());
assert(file);
serializeASCII(mat, file);
file.close();
}
template<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* mat)
{
assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0);
std::ifstream file;
file.open(filename.c_str());
if(!file)
std::cerr << "File " << filename << " could not be opened. Dying badly." << std::endl;
assert(file);
deserializeASCII(file, mat);
file.close();
}
template<class T>
void serializeScalar(T val, std::ostream& strm)
{
strm.write((char*)&val, sizeof(T));
}
template<class T>
void deserializeScalar(std::istream& strm, T* val)
{
strm.read((char*)val, sizeof(T));
}
}
#endif // EIGEN_EXTENSIONS_H
corelib/src/clams/frame_projector.cpp
+241
View File
@@ -0,0 +1,241 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace Eigen;
namespace clams
{
FrameProjector::FrameProjector(const rtabmap::CameraModel & model) :
model_(model)
{
UASSERT(model.isValidForReprojection());
}
// pcd is in /map frame
FrameProjector::RangeIndex FrameProjector::cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const
{
int height = model_.imageHeight();
int width = model_.imageWidth();
RangeIndex ind;
if((int)ind.size() != height)
ind.resize(height);
for(size_t y = 0; y < ind.size(); ++y)
if((int)ind[y].size() != width)
ind[y].resize(width);
for(size_t y = 0; y < ind.size(); ++y) {
for(size_t x = 0; x < ind[y].size(); ++x) {
ind[y][x].clear();
ind[y][x].reserve(10);
}
}
rtabmap::Transform t = model_.localTransform().inverse();
ProjectivePoint ppt;
for(size_t i = 0; i < pcd->size(); ++i) {
if(!isFinite(pcd->at(i)))
continue;
ppt = reproject(rtabmap::util3d::transformPoint(pcd->at(i), t));
if(ppt.z_ == 0 || !(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < width && ppt.v_ < height))
continue;
ind[ppt.v_][ppt.u_].push_back(ppt.z_);
}
return ind;
}
pcl::PointXYZ FrameProjector::project(const ProjectivePoint& ppt) const
{
UASSERT(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < model_.imageWidth() && ppt.v_ < model_.imageHeight());
pcl::PointXYZ pt;
model_.project(ppt.u_, ppt.v_, ppt.z_, pt.x, pt.y, pt.z);
return pt;
}
ProjectivePoint FrameProjector::reproject(const pcl::PointXYZ & pt) const
{
UASSERT(isFinite(pt));
ProjectivePoint ppt;
if(pt.z > 0)
{
model_.reproject(pt.x, pt.y, pt.z, ppt.u_, ppt.v_);
ppt.z_ = pt.z;
}
return ppt;
}
cv::Mat FrameProjector::estimateMapDepth(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
const rtabmap::Transform & transform,
const cv::Mat& measurement,
double coneRadius,
double coneStdevThresh) const
{
cv::Mat estimate = cv::Mat::zeros(measurement.size(), CV_32FC1);
// -- Get the depth index.
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = rtabmap::util3d::transformPointCloud(map, transform);
RangeIndex rindex = cloudToRangeIndex(transformed);
// -- Compute the edge-of-map mask.
cv::Mat naive_mapframe = rtabmap::util3d::projectCloudToCamera(model_.imageSize(), model_.K(), transformed, model_.localTransform());
const cv::Mat& measurement_depth = measurement;
const cv::Mat& naive_mapdepth = naive_mapframe;
cv::Mat1b mask(measurement_depth.rows, measurement_depth.cols);
mask = 0;
for(int y = 0; y < mask.rows; ++y)
for(int x = 0; x < mask.cols; ++x)
if(naive_mapdepth.at<float>(y, x) != 0)
mask(y, x) = 255;
cv::dilate(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 15);
bool isInMM = measurement_depth.type() == CV_16UC1;
// -- Main loop: for all points in the image...
ProjectivePoint ppt;
for(ppt.v_ = 0; ppt.v_ < measurement_depth.rows; ++ppt.v_) {
for(ppt.u_ = 0; ppt.u_ < measurement_depth.cols; ++ppt.u_) {
float value = isInMM?float(measurement_depth.at<unsigned short>(ppt.v_, ppt.u_))*0.001f:measurement_depth.at<float>(ppt.v_, ppt.u_);
// -- Reject points with no data.
if(value == 0)
continue;
if(naive_mapdepth.at<float>(ppt.v_, ppt.u_) == 0)
continue;
// -- Reject points on the edge of the map.
if(mask(ppt.v_, ppt.u_) == 0)
continue;
// -- Find nearby points in the cone to get a good estimate of the map depth.
double mean = 0;
double stdev = 0;
//double stdev_thresh = numeric_limits<double>::max();
bool valid = coneFit(
naive_mapdepth.size(),
rindex,
ppt.u_,
ppt.v_,
coneRadius,
value,
&mean,
&stdev);
if(!valid)
continue;
if(stdev > coneStdevThresh)
continue;
estimate.at<float>(ppt.v_, ppt.u_) = mean;
}
}
return estimate;
}
bool FrameProjector::coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
int uc, int vc, double radius, double measurement_depth,
double* mean, double* stdev) const
{
pcl::PointXYZ pt_center, pt_ul, pt_lr;
ProjectivePoint ppt, ppt_ul, ppt_lr;
ppt.u_ = uc;
ppt.v_ = vc;
ppt.z_ = measurement_depth;
pt_center = project(ppt);
pt_ul = pt_center;
pt_lr = pt_center;
pt_ul.x -= radius;
pt_ul.y -= radius;
pt_lr.x += radius;
pt_lr.y += radius;
ppt_ul = reproject(pt_ul);
ppt_lr = reproject(pt_lr);
if(ppt_ul.z_ == 0 || !(ppt_ul.u_ >= 0 && ppt_ul.v_ >= 0 && ppt_ul.u_ < imageSize.width && ppt_ul.v_ < imageSize.height))
return false;
if(ppt_lr.z_ == 0 || !(ppt_lr.u_ >= 0 && ppt_lr.v_ >= 0 && ppt_lr.u_ < imageSize.width && ppt_lr.v_ < imageSize.height))
return false;
int min_u = ppt_ul.u_;
int max_u = ppt_lr.u_;
int min_v = ppt_ul.v_;
int max_v = ppt_lr.v_;
*mean = 0;
double num = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT) {
*mean += vals[i];
++num;
}
}
}
}
if(num == 0)
return false;
*mean /= num;
double var = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT)
var += (vals[i] - *mean) * (vals[i] - *mean);
}
}
}
var /= num;
*stdev = sqrt(var);
return true;
}
} // namespace clams
corelib/src/clams/slam_calibrator.cpp
+78
View File
@@ -0,0 +1,78 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
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 <organization> 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 <COPYRIGHT HOLDER> 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.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/slam_calibrator.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteDepthDistortionModel calibrate(
const std::map<int, rtabmap::SensorData> & sequence,
const std::map<int, rtabmap::Transform> & trajectory,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
double coneRadius,
double coneStdevThresh)
{
DiscreteDepthDistortionModel model;
if(!sequence.empty())
{
const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
model = DiscreteDepthDistortionModel(imageSize.width, imageSize.height);
// -- For all selected frames, accumulate training examples
// in the distortion model.
size_t counts;
#pragma omp parallel for
for(std::map<int, rtabmap::Transform>::const_iterator iter = trajectory.begin(); iter != trajectory.end(); ++iter)
{
size_t idx = iter->first;
std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
if(ster!=sequence.end())
{
cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage);
cv::Mat mapDepth;
FrameProjector projector(ster->second.cameraModels()[0]);
mapDepth = projector.estimateMapDepth(map, iter->second.inverse(), depthImage, coneRadius, coneStdevThresh);
counts = model.accumulate(mapDepth, depthImage);
}
}
UINFO("counts=%d", (int)counts);
}
return model;
}
} // namespace clams
corelib/src/util3d.cpp
+18 -13
View File
@@ -1356,9 +1356,9 @@ cv::Point3f projectDisparityTo3D(
// Register point cloud to camera (return registered depth image)
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & cameraMatrixK,
const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan.empty());
@@ -1421,9 +1421,9 @@ cv::Mat projectCloudToCamera(
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & cameraMatrixK,
const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan->empty());
@@ -1446,17 +1446,22 @@ cv::Mat projectCloudToCamera(
// re-project in camera frame
float z = ptScan.z;
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
int dy = (fy*ptScan.y)*invZ + cy;
if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
if(z > 0.0f)
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
if(uIsInBounds(dx, 0, registered.cols))
{
zReg = z;
int dy = (fy*ptScan.y)*invZ + cy;
if(uIsInBounds(dy, 0, registered.rows))
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
}
}