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

4
corelib/include/rtabmap/core/CameraInfo.h
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;
};

10
corelib/include/rtabmap/core/CameraModel.h
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_;

9
corelib/include/rtabmap/core/CameraThread.h
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

130
corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h Normal file
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

96
corelib/include/rtabmap/core/clams/frame_projector.h Normal file
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

52
corelib/include/rtabmap/core/clams/slam_calibrator.h Normal file
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