/********************************************************************
** Image Component Library (ICL) **
** **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld **
** Neuroinformatics Group **
** Website: www.iclcv.org and **
** http://opensource.cit-ec.de/projects/icl **
** **
** File : ICLGeom/src/ICLGeom/RansacBasedPoseEstimator.cpp **
** Module : ICLGeom **
** Authors: Christof Elbrechter **
** **
** **
** GNU LESSER GENERAL PUBLIC LICENSE **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the **
** **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file. Please review the **
** following information to ensure the license requirements will **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt **
** **
** The development of this software was supported by the **
** Excellence Cluster EXC 277 Cognitive Interaction Technology. **
** The Excellence Cluster EXC 277 is a grant of the Deutsche **
** Forschungsgemeinschaft (DFG) in the context of the German **
** Excellence Initiative. **
** **
********************************************************************/
#include <ICLGeom/RansacBasedPoseEstimator.h>
#include <ICLUtils/Uncopyable.h>
#include <ICLUtils/StringUtils.h>
#include <ICLMath/RansacFitter.h>
#include <ICLGeom/CoplanarPointPoseEstimator.h>
namespace icl{
namespace geom{
using namespace math;
using namespace utils;
struct RansacBasedPoseEstimator::Data{
// int iterations;
//int minPoints;
//float maxError;
//float minPointsForGoodModel;
Camera camera;
CoplanarPointPoseEstimator pe;
std::vector<utils::Point32f> lastConsensusSet;
};
static icl64f err_coplanar_verbose(const std::vector<float> &m, const std::vector<float> &p, const Camera &cam){
std::cout << "model: ["
<< m[0] << ", "
<< m[1] << ", "
<< m[2] << ", "
<< m[3] << ", "
<< m[4] << ", "
<< m[5] << "]" << std::endl;
Mat T = create_hom_4x4<float>(m[0],m[1],m[2],m[3],m[4],m[5]);
SHOW(T);
SHOW(T * Vec(p[2],p[3],0,1));
Point32f q = cam.project(T * Vec(p[2], p[3], 0, 1));
SHOW(q);
icl64f err = sqr(p[0] - q[0]) + sqr(p[1] - q[1]);
return err;
}
icl64f RansacBasedPoseEstimator::err_coplanar(const std::vector<float> &m, const std::vector<float> &p){
Mat T = create_hom_4x4<float>(m[0],m[1],m[2],m[3],m[4],m[5]);
Point32f q = m_data->camera.project(T * Vec(p[2], p[3], 0, 1));
icl64f err = sqr(p[0] - q[0]) + sqr(p[1] - q[1]);
return err;
}
static icl64f mean_error(const std::vector<std::vector<float> > &pts, const std::vector<float> &m,
Function<icl64f,const std::vector<float> &, const std::vector<float> &> err_coplanar,
const Camera &cam){
//std::cout << "mean error ";
icl64f err = 0;
for(size_t i=0;i<pts.size();++i){
err += err_coplanar(m,pts[i]);
//if(str(err) == "nan" || str(err) == "-nan") {
// err_coplanar_verbose(m,pts[i], cam);
// break;
//}
}
//std::cout << "mean error end ";
return err / pts.size();
}
std::vector<float> RansacBasedPoseEstimator::fit_coplanar(const std::vector<std::vector<float> > &pts){
//DEBUG_LOG("------------- fitting on " << pts.size() << " point");
//std::cout << "Points:: [";
std::vector<Point32f> curr(pts.size()),templ(pts.size());
for(size_t i=0;i<pts.size();++i){
curr[i] = Point32f(pts[i][0], pts[i][1]);
templ[i] = Point32f(pts[i][2], pts[i][3]);
//std::cout << curr[i] << "->" << templ[i] << (i < pts.size()-1 ? ",": "]\n");
}
Mat T = m_data->pe.getPose(pts.size(), templ.data(), curr.data(), m_data->camera);
//SHOW(T);
Vec3 e = extract_euler_angles(T);
std::vector<float> p(6);
p[0] = e[0];
p[1] = e[1];
p[2] = e[2];
p[3] = T(3,0);
p[4] = T(3,1);
p[5] = T(3,2);
/*
std::cout << "model: ["
<< p[0] << ", "
<< p[1] << ", "
<< p[2] << ", "
<< p[3] << ", "
<< p[4] << ", "
<< p[5] << "] " << std::endl;
std::cout << "(Err: "
<< mean_error(pts, p, function(this,&RansacBasedPoseEstimator::err_coplanar), m_data->camera) << ")"<< std::endl;
*/
return p;
}
RansacBasedPoseEstimator::RansacBasedPoseEstimator(const geom::Camera &camera,
int iterations,
int minPoints,
float maxErr,
float minPointsForGoodModel,
bool storeLastConsensusSet){
m_data = new Data;
addProperty("iterations","range","[1,1000000]:1",str(iterations));
addProperty("min points","range","[1,1000000]:1",str(minPoints));
addProperty("max error","range","[0,10e30]",str(maxErr));
addProperty("min points for good model", "range", "[0,10000000]:1",str(minPointsForGoodModel));
addProperty("debug output","flag","",false);
addProperty("store last consensus set","flag","",storeLastConsensusSet);
addChildConfigurable(&m_data->pe,"pose estimator");
setPropertyValue("pose estimator.algorithm","HomographyBasedOnly");
m_data->camera = camera;
}
void RansacBasedPoseEstimator::setStoreLastConsensusSet(bool on){
setPropertyValue("store last consensus set",on);
}
std::vector<utils::Point32f> RansacBasedPoseEstimator::getLastConsensusSet() throw (utils::ICLException){
bool hasSet = getPropertyValue("store last consensus set");
if(!hasSet) throw utils::ICLException("RansacBasedPoseEstimator::getLastConsensusSet() even though "
"'store last consensus set' property was not set to 'true'");
return m_data->lastConsensusSet;
}
RansacBasedPoseEstimator::~RansacBasedPoseEstimator(){
delete m_data;
}
void RansacBasedPoseEstimator::setIterations(int iterations){
setPropertyValue("iterations",iterations);
}
void RansacBasedPoseEstimator::setMinPoints(int minPoints){
setPropertyValue("min points",minPoints);
}
void RansacBasedPoseEstimator::setMaxError(float maxError){
setPropertyValue("max error",maxError);
}
void RansacBasedPoseEstimator::setMinPointsForGoodModel(float f){
setPropertyValue("min points for good model", f);
}
RansacBasedPoseEstimator::Result
RansacBasedPoseEstimator::fit(const std::vector<Point32f> &templ,
const std::vector<Point32f> &curr){
int iterations = getPropertyValue("iterations");
int maxError = getPropertyValue("max error");
int minPoints = getPropertyValue("min points");
int minPointsForGoodModel = getPropertyValue("min points for good model");
bool dbg = getPropertyValue("debug output");
RansacFitter<> ransac(minPoints, iterations,
function(this,&RansacBasedPoseEstimator::fit_coplanar),
function(this,&RansacBasedPoseEstimator::err_coplanar),
maxError, minPointsForGoodModel);
RansacFitter<>::DataSet data(curr.size(), std::vector<float>(4,0));
for(size_t i=0;i<curr.size();++i){
data[i][0] = curr[i].x;
data[i][1] = curr[i].y;
data[i][2] = templ[i].x;
data[i][3] = templ[i].y;
//if(dbg){
//DEBUG_LOG("associating curr: " << curr[i] << " --> " << templ[i]);
//}
}
if(dbg){
DEBUG_LOG("using these parameters:");
SHOW(minPoints);
SHOW(iterations);
SHOW(maxError);
SHOW(minPointsForGoodModel);
SHOW(curr.size());
SHOW(data.size());
}
const RansacFitter<>::Result &res = ransac.fit(data);
if(getPropertyValue("store last consensus set")){
m_data->lastConsensusSet.resize(res.consensusSet.size());
for(size_t i=0;i<m_data->lastConsensusSet.size();++i){
m_data->lastConsensusSet[i] = Point32f(res.consensusSet[i][0], res.consensusSet[i][1]);
}
}
if(res.found()){
const std::vector<float> &m = res.model;
Mat T = create_hom_4x4<float>(m[0],m[1],m[2],m[3],m[4],m[5]);
if(dbg){
DEBUG_LOG("result was found: \n" << T << "\n");
}
Result r = { T, true, (float)res.error };
return r;
}else{
if(dbg){
DEBUG_LOG("result was not found");
}
}
Result r = { Mat::id(), false, float(-1) };
return r;
}
RansacBasedPoseEstimator::Result
RansacBasedPoseEstimator::fit(const std::vector<Vec> &modelPoints,
const std::vector<Point32f> &imagePoints){
throw ICLException("RansacBasedPoseEstimator::fit is not yet implemented for non-planar targets");
Result r = { Mat::id(), false, float(-1) };
return r;
}
}
}