void KMEANS<T>::print()
{
ofstream fout;
fout.open("res.txt");
if (!fout)
{
cout << "file res.txt open failed" << endl;
exit(0);
}
typename vector< vector<T> >::iterator it = dataSet.begin();
typename vector< tNode >::iterator itt = clusterAssignment.begin();
for (int i = 0; i < rowLen; ++i)
{
typename vector<T>::iterator it2 = it->begin();
while ( it2 != it->end() )
{
fout << *it2 << "\t";
++it2;
}
fout << (*itt).minIndex << endl;
++itt;
++it;
}
}
int Polynomial<CoefficientType>::getDegree() const {
int max_degree = 0;
for (typename vector<Monomial>::const_iterator iter=monomials.begin(); iter!=monomials.end(); iter++) {
int monomial_degree = iter->getDegree();
if (monomial_degree > max_degree) max_degree = monomial_degree;
}
return max_degree;
}
Error_PN2S NetworkAnalyzer::importCompts(vector<models::Compartment > &cmpts)
{
typename vector<models::Compartment >::iterator n;
for(n = cmpts.begin(); n != cmpts.end(); ++n) {
allCompartments.push_back(n.base());
// importHHChannels(n->hhchannels);
}
return Error_PN2S::NO_ERROR;
}
Error_PN2S NetworkAnalyzer::importHHChannels(vector<models::HHChannel > &chs)
{
if(chs.size() > 0)
{
typename vector<models::HHChannel >::iterator n;
for(n = chs.begin(); n != chs.end(); ++n) {
allHHChannels.push_back(n.base());
}
}
return Error_PN2S::NO_ERROR;
}
bool CellMap::removeEntity (Entity *entity) {
typename vector<std::unique_ptr<Entity>>::iterator it;
for (it = entityInside.begin(); it != entityInside.end();) {
if (entity == it->get()) {
it->release();
it = entityInside.erase(it);
return true;
} else
it++;
}
return false;
}
virtual int thread_local_alloc (void (*dtor)(intptr_t))
{
int index = (int)entries_.size();
entries_.resize(index + 1);
entry& ent = entries_[index];
ent.alive_ = true;
ent.dtor_ = dtor;
for (size_t i = 0; i != thread_count; ++i)
{
ent.value_[i] = 0;
}
return index;
}
void
State_t<EVENT>::operator-=(const State_t<EVENT>&s)
{
for (auto & elem : s._arcs) {
typename vector<arc_type>::reverse_iterator j;
for (j = _arcs.rbegin(); j != _arcs.rend(); ++j) {
if (j->match(elem.event())) {
_arcs.erase(j.base() - 1);
break;
}
}
}
}
virtual intptr_t thread_local_get (int index)
{
RL_VERIFY(index >= 0 && (size_t)index < entries_.size());
entry& ent = entries_[index];
RL_VERIFY(ent.alive_);
return ent.value_[current_thread()];
}
virtual void thread_local_set (int index, intptr_t value)
{
RL_VERIFY(index >= 0 && (size_t)index < entries_.size());
entry& ent = entries_[index];
RL_VERIFY(ent.alive_);
ent.value_[current_thread()] = value;
}
template<class T> void printArray2D(vector< vector<T> > &I)
{
// This is how we iterate using an iterator for 2d vectors
typename vector< vector<T> >::iterator row; // Iterator for row of 2d vector
typename vector<T>::iterator col; // Iterator for columns of 2d vector
cout << "Matrix size: " << "[" << I.size() << "x" << I[0].size() << "]" << endl; // print the row and columns
for(row = I.begin(); row !=I.end(); row++)
{
for(col = row->begin(); col != row->end(); col++)
{
cout << *col << ","; // print the value contained in each row of our 2d vector.
}
cout << endl;
}
}
void Node<D>:: bulkLoading(vector<Dados<D> > dados, int ordem){
typename vector<Dados<D> >::iterator atual = dados.begin();
Node<D> *novo = new Node<D>(ordem);
novo->folha = true;
Node<D> *first = novo;
Node<D> *aux;
for(typename vector<Dados<D> >::iterator it=dados.begin()+1; it!=dados.end(); ++it ){
if(*atual==*it){
atual->addRef( it->getOffsets().back() );
it->clearOffsets();
}else{
unsigned int a = atual-dados.begin();
novo->chaves.push_back(dados[a]);
atual = it ;
if(novo->chaves.size()>=(unsigned int)(ordem-1)/2){
aux = novo;
novo = new Node<D>(ordem);
novo->folha = true;
aux->prox = novo;
}
}
}
novo->prox = (Node<D>*)NULL;
aux->prox = (Node<D>*)NULL;
novo = first;
while(novo->prox != (Node<D>*)NULL ){
for(unsigned int i = 0; i<novo->chaves.size() && novo->folha; i++){
#ifdef DEBUG
cout<<novo->chaves[i]<<endl;
#endif
Node<D>::inserePai( novo, novo->prox);
novo = novo->prox;
}
}
// for( vector<Dados<D> >::iterator it=dados.begin(); it!=dados.end(); ++it )
// if(!it->getOffsets().empty())
// cout<<*it<<endl;
};
void FeatureGroupingAlgorithmQT::group_(const vector<MapType>& maps,
ConsensusMap& out)
{
// check that the number of maps is ok:
if (maps.size() < 2)
{
throw Exception::IllegalArgument(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION,
"At least two maps must be given!");
}
QTClusterFinder cluster_finder;
cluster_finder.setParameters(param_.copy("", true));
cluster_finder.run(maps, out);
StringList ms_run_locations;
// add protein IDs and unassigned peptide IDs to the result map here,
// to keep the same order as the input maps (useful for output later):
for (typename vector<MapType>::const_iterator map_it = maps.begin();
map_it != maps.end(); ++map_it)
{
// add protein identifications to result map:
out.getProteinIdentifications().insert(
out.getProteinIdentifications().end(),
map_it->getProteinIdentifications().begin(),
map_it->getProteinIdentifications().end());
// add unassigned peptide identifications to result map:
out.getUnassignedPeptideIdentifications().insert(
out.getUnassignedPeptideIdentifications().end(),
map_it->getUnassignedPeptideIdentifications().begin(),
map_it->getUnassignedPeptideIdentifications().end());
}
// canonical ordering for checking the results:
out.sortByQuality();
out.sortByMaps();
out.sortBySize();
return;
}
IGL_INLINE void igl::polygon_mesh_to_triangle_mesh(
const std::vector<std::vector<Index> > & vF,
Eigen::PlainObjectBase<DerivedF>& F)
{
using namespace std;
using namespace Eigen;
int m = 0;
// estimate of size
for(typename vector<vector<Index > >::const_iterator fit = vF.begin();
fit!=vF.end();
fit++)
{
if(fit->size() >= 3)
{
m += fit->size() - 2;
}
}
// Resize output
F.resize(m,3);
{
int k = 0;
for(typename vector<vector<Index > >::const_iterator fit = vF.begin();
fit!=vF.end();
fit++)
{
if(fit->size() >= 3)
{
typename vector<Index >::const_iterator cit = fit->begin();
cit++;
typename vector<Index >::const_iterator pit = cit++;
for(;
cit!=fit->end();
cit++,pit++)
{
F(k,0) = *(fit->begin());
F(k,1) = *pit;
F(k,2) = *cit;
k++;
}
}
}
assert(k==m);
}
}
virtual void thread_local_free (int index)
{
RL_VERIFY(index >= 0 && (size_t)index < entries_.size());
entry& ent = entries_[index];
RL_VERIFY(ent.alive_);
ent.alive_ = false;
if (ent.dtor_)
{
for (size_t i = 0; i != thread_count; ++i)
{
if (ent.value_[i])
{
ent.dtor_(ent.value_[i]);
}
}
}
}
void switchScores_(IDType& id, Size& counter)
{
for (typename vector<typename IDType::HitType>::iterator hit_it = id.getHits().begin();
hit_it != id.getHits().end(); ++hit_it, ++counter)
{
if (!hit_it->metaValueExists(new_score_))
{
String msg = "Meta value '" + new_score_ + "' not found for " +
describeHit_(*hit_it);
throw Exception::MissingInformation(__FILE__, __LINE__,
__PRETTY_FUNCTION__, msg);
}
String old_score_meta = (old_score_.empty() ? id.getScoreType() :
old_score_);
DataValue dv = hit_it->getMetaValue(old_score_meta);
if (!dv.isEmpty()) // meta value for old score already exists
{
if (fabs((double(dv) - hit_it->getScore()) * 2.0 /
(double(dv) + hit_it->getScore())) > tolerance_)
{
String msg = "Meta value '" + old_score_meta + "' already exists "
"with a conflicting value for " + describeHit_(*hit_it);
throw Exception::InvalidValue(__FILE__, __LINE__, __PRETTY_FUNCTION__,
msg, dv.toString());
} // else: values match, nothing to do
}
else
{
hit_it->setMetaValue(old_score_meta, hit_it->getScore());
}
hit_it->setScore(hit_it->getMetaValue(new_score_));
}
id.setScoreType(new_score_type_);
id.setHigherScoreBetter(higher_better_);
}
void test_createFilters(const Kernel& f)
{
using namespace MatchedFilter;
if (os_) *os_ << "test_createFilters() " << typeid(f).name() << endl;
int sampleRadius = 2;
int subsampleFactor = 4;
double dx = 1;
typedef typename KernelTraits<Kernel>::filter_type filter_type;
typedef typename KernelTraits<Kernel>::ordinate_type ordinate_type;
vector<filter_type> filters = details::createFilters(f,
sampleRadius,
subsampleFactor,
dx);
// verify filter count
unit_assert((int)filters.size() == subsampleFactor);
for (typename vector<filter_type>::const_iterator it=filters.begin(); it!=filters.end(); ++it)
{
if (os_)
{
copy(it->begin(), it->end(), ostream_iterator<ordinate_type>(*os_, " "));
*os_ << endl;
}
// verify filter size
unit_assert((int)it->size() == sampleRadius*2 + 1);
// verify filter normalization
double sum = 0;
for (typename filter_type::const_iterator jt=it->begin(); jt!=it->end(); ++jt)
sum += norm(complex<double>(*jt));
unit_assert_equal(sum, 1, 1e-14);
}
if (os_) *os_ << endl;
}
void ListPorts(const vector<PortClass> &ports, bool input) {
typename vector<PortClass>::const_iterator port_iter;
for (port_iter = ports.begin(); port_iter != ports.end(); ++port_iter) {
cout << " port " << port_iter->Id() << ", ";
if (input)
cout << "IN";
else
cout << "OUT";
if (!port_iter->Description().empty()) {
cout << " " << port_iter->Description();
}
switch (port_iter->PriorityCapability()) {
case ola::CAPABILITY_STATIC:
cout << ", priority " << static_cast<int>(port_iter->Priority());
break;
case ola::CAPABILITY_FULL:
cout << ", priority ";
if (port_iter->PriorityMode() == ola::PRIORITY_MODE_INHERIT)
cout << "inherited";
else
cout << "overide " << static_cast<int>(port_iter->Priority());
break;
default:
break;
}
if (port_iter->IsActive())
cout << ", patched to universe " << port_iter->Universe();
if (port_iter->SupportsRDM())
cout << ", RDM supported";
cout << endl;
}
}
void nl_shinji_kneip_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter,
const POINT_T thre_3d_, const POINT_T thre_2d_, const POINT_T nl_thre, int& Iter, POINT_T confidence = 0.99){
typedef Matrix<POINT_T, Dynamic, Dynamic> MX;
typedef Matrix<POINT_T, 3, 1> P3;
typedef SE3Group<POSE_T> RT;
POSE_T cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
POINT_T cos_nl_thre = cos(nl_thre);
RandomElements<int> re((int)adapter.getNumberCorrespondences());
const int K = 3;
MX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
MX Nw(3, K + 1), Nc(3, K + 1);
Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);
adapter.setMaxVotes(-1);
for (int ii = 0; ii < Iter; ii++) {
//randomly select K candidates
RT solution_kneip, solution_shinji, solution_nl;
vector<RT> v_solutions;
vector<int> selected_cols;
re.run(K + 1, &selected_cols);
if (assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
solution_shinji = shinji<POSE_T, POINT_T>(Xw, Xc, K);
v_solutions.push_back(solution_shinji);
}
if (kneip<POSE_T, POINT_T>(Xw, bv, &solution_kneip)){
v_solutions.push_back(solution_kneip);
}
nl_2p<POSE_T, POINT_T>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
v_solutions.push_back(solution_nl);
for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
//collect votes
int votes = 0;
inliers.setZero();
P3 eivE; P3 pc; POINT_T cos_a;
for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
if (adapter.isValid(c)){
//with normal data
POINT_T cos_alpha = adapter.getNormalCurr(c).dot(itr->so3().template cast<POINT_T>() * adapter.getNormalGlob(c));
if (cos_alpha > cos_nl_thre){
inliers(c, 2) = 1;
votes++;
}
//with 3d data
eivE = adapter.getPointCurr(c) - (itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>());
if (eivE.norm() < thre_3d_){
inliers(c, 1) = 1;
votes++;
}
}
//with 2d
pc = itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>();// transform pw into pc
pc = pc / pc.norm(); //normalize pc
//compute the score
cos_a = pc.dot( adapter.getBearingVector(c) );
if (cos_a > cos_thr){
inliers(c, 0) = 1;
votes++;
}
}
//cout << endl;
if (votes > adapter.getMaxVotes() ){
assert(votes == inliers.sum());
adapter.setMaxVotes(votes);
adapter.setRcw(itr->so3());
adapter.sett(itr->translation());
adapter.setInlier(inliers);
//cout << inliers.inverse() << endl << endl;
//adapter.printInlier();
//Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 3 - votes) / adapter.getNumberCorrespondences() / 3, K, Iter);
}
}//for(vector<RT>::iterator itr = v_solutions.begin() ...
}//for(int ii = 0; ii < Iter; ii++)
PnPPoseAdapter<POSE_T, POINT_T>* pAdapterPnP = &adapter;
pAdapterPnP->cvtInlier();
AOPoseAdapter<POSE_T, POINT_T>* pAdapterAO = &adapter;
pAdapterAO->cvtInlier();
adapter.cvtInlier();
return;
}
#ifndef BTL_AO_NORM_POSE_HEADER
#define BTL_AO_NORM_POSE_HEADER
#include "common/OtherUtil.hpp"
#include <limits>
#include <Eigen/Dense>
#include "NormalAOPoseAdapter.hpp"
#include "AbsoluteOrientation.hpp"
using namespace Eigen;
using namespace std;
template<typename POSE_T, typename POINT_T>
Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
typedef Matrix<POSE_T, 3, 1> V3;
typedef Matrix<POSE_T, 3, 3> M3;
M3 Rwc = adapter.getRcw().inverse().matrix();
M3 AA; AA.setZero();
V3 bb; bb.setZero();
for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
{
if (adapter.isInlier23(i)){
V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
M3 A;
A(0,0) = 1 - vr_w(0)*vr_w(0);
A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
A(1,1) = 1 - vr_w(1)*vr_w(1);
A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
A(2,2) = 1 - vr_w(2)*vr_w(2);
V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
AA += A;
bb += b;
}
}
V3 c_w;
if (fabs(AA.determinant()) < POSE_T(0.0001))
c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
else
c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
return c_w;
}
template< typename POSE_T, typename POINT_T >
bool assign_sample(const NormalAOPoseAdapter<POSE_T, POINT_T>& adapter,
const vector<int>& selected_cols_,
Matrix<POINT_T, Dynamic, Dynamic>* p_X_w_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_w_,
Matrix<POINT_T, Dynamic, Dynamic>* p_X_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_N_c_, Matrix<POINT_T, Dynamic, Dynamic>* p_bv_){
int K = (int)selected_cols_.size() - 1;
bool use_shinji = false;
int nValid = 0;
for (int nSample = 0; nSample < K; nSample++) {
p_X_w_->col(nSample) = adapter.getPointGlob(selected_cols_[nSample]);
p_N_w_->col(nSample) = adapter.getNormalGlob(selected_cols_[nSample]);
p_bv_->col(nSample) = adapter.getBearingVector(selected_cols_[nSample]);
if (adapter.isValid(selected_cols_[nSample])){
p_X_c_->col(nSample) = adapter.getPointCurr(selected_cols_[nSample]);
p_N_c_->col(nSample) = adapter.getNormalCurr(selected_cols_[nSample]);
nValid++;
}
}
if (nValid == K)
use_shinji = true;
//assign the fourth elements for
p_X_w_->col(3) = adapter.getPointGlob(selected_cols_[3]);
p_N_w_->col(3) = adapter.getNormalGlob(selected_cols_[3]);
p_bv_->col(3) = adapter.getBearingVector(selected_cols_[3]);
return use_shinji;
}
template<typename POSE_T, typename POINT_T>
void nl_2p( const Matrix<POINT_T,3,1>& pt1_c, const Matrix<POINT_T,3,1>& nl1_c, const Matrix<POINT_T,3,1>& pt2_c,
const Matrix<POINT_T,3,1>& pt1_w, const Matrix<POINT_T,3,1>& nl1_w, const Matrix<POINT_T,3,1>& pt2_w,
SE3Group<POSE_T>* p_solution){
//Drost, B., Ulrich, M., Navab, N., & Ilic, S. (2010). Model globally, match locally: Efficient and robust 3D object recognition. In CVPR (pp. 998?005). Ieee. http://doi.org/10.1109/CVPR.2010.5540108
typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
typedef Matrix<POSE_T, 3, 1> V3;
typedef SO3Group<POSE_T> ROTATION;
typedef SE3Group<POSE_T> RT;
V3 c_w = pt1_w.template cast<POSE_T>(); // c_w is the origin of coordinate g w.r.t. world
POSE_T alpha = acos(nl1_w(0)); // rotation nl1_c to x axis (1,0,0)
V3 axis( 0, nl1_w(2), -nl1_w(1)); //rotation axis between nl1_c to x axis (1,0,0) i.e. cross( nl1_w, x );
axis.normalized();
//verify quaternion and rotation matrix
Quaternion<POSE_T> q_g_f_w(AngleAxis<POSE_T>(alpha, axis));
//cout << q_g_f_w << endl;
ROTATION R_g_f_w(q_g_f_w);
//cout << R_g_f_w << endl;
V3 nl_x = R_g_f_w * nl1_w.template cast<POSE_T>();
axis.normalized();
V3 c_c = pt1_c.template cast<POSE_T>();
POSE_T beta = acos(nl1_c(0)); //rotation nl1_w to x axis (1,0,0)
V3 axis2( 0, nl1_c(2), -nl1_c(1) ); //rotation axis between nl1_m to x axis (1,0,0) i.e. cross( nl1_w, x );
axis2.normalized();
Quaternion<POSE_T> q_gp_f_c(AngleAxis<POSE_T>(beta, axis2));
//cout << q_gp_f_c << endl;
ROTATION R_gp_f_c(q_gp_f_c);
//cout << R_gp_f_c << endl;
//{
// Vector3 nl_x = R_gp_f_c * nl1_c;
// print<T, Vector3>(nl_x);
//}
V3 pt2_g = R_g_f_w * (pt2_w.template cast<POSE_T>() - c_w); pt2_g(0) = POINT_T(0); pt2_g.normalized();
V3 pt2_gp = R_gp_f_c * (pt2_c.template cast<POSE_T>() - c_c); pt2_gp(0) = POINT_T(0); pt2_gp.normalized();
POSE_T gamma = acos(pt2_g.dot(pt2_gp)); //rotate pt2_g to pt2_gp;
V3 axis3(1,0,0);
Quaternion< POSE_T > q_gp_f_g(AngleAxis<POSE_T>(gamma, axis3));
//cout << q_gp_f_g << endl;
ROTATION R_gp_f_g ( q_gp_f_g );
//cout << R_gp_f_g << endl;
ROTATION R_c_f_gp = R_gp_f_c.inverse();
p_solution->so3() = R_c_f_gp * R_gp_f_g * R_g_f_w;
//{
// T3 pt = *R_cfw * (pt2_w - c_w) + c_c;
// cout << norm<T, T3>( pt - pt2_c ) << endl;
//}
//{
// cout << norm<T, T3>(nl1_w) << endl;
// cout << norm<T, T3>(nl1_c) << endl;
// cout << norm<T, T3>(*R_cfw * nl1_w) << endl;
// cout << norm<T, T3>(nl1_c - *R_cfw * nl1_w) << endl;
//}
p_solution->translation() = c_c - p_solution->so3() * c_w;
return;
}
template< typename POSE_T, typename POINT_T > /*Matrix<float,-1,-1,0,-1,-1> = MatrixXf*/
SE3Group<POSE_T> nl_shinji_ls(const Matrix<POINT_T, Dynamic, Dynamic> & Xw_, const Matrix<POINT_T, Dynamic, Dynamic>& Xc_,
const Matrix<POINT_T, Dynamic, Dynamic> & Nw_, const Matrix<POINT_T, Dynamic, Dynamic>& Nc_, const int K) {
typedef SE3Group<POSE_T> RT;
assert(Xw_.rows() == 3);
//Compute the centroid of each point set
Matrix<POINT_T, 3, 1> Cw(0, 0, 0), Cc(0, 0, 0); //Matrix<float,3,1,0,3,1> = Vector3f
for (int nCount = 0; nCount < K; nCount++){
Cw += Xw_.col(nCount);
Cc += Xc_.col(nCount);
}
Cw /= (POINT_T)K;
Cc /= (POINT_T)K;
// transform coordinate
Matrix<POSE_T, 3, 1> Aw, Ac;
Matrix<POSE_T, 3, 3> M; M.setZero();
Matrix<POSE_T, 3, 3> N;
POSE_T sigma_w_sqr = 0;
for (int nC = 0; nC < K; nC++){
Aw = Xw_.col(nC) - Cw; sigma_w_sqr += Aw.squaredNorm();
Ac = Xc_.col(nC) - Cc;
N = Ac * Aw.transpose();
M += N;
}
M /= (POSE_T)K;
sigma_w_sqr /= (POSE_T)K;
Matrix<POSE_T, 3, 3> M_n; M_n.setZero();
for (int nC = 0; nC < K; nC++){
//pure imaginary Shortcuts
Aw = Nw_.col(nC);
Ac = Nc_.col(nC);
N = Ac * Aw.transpose();
M_n += (sigma_w_sqr*N);
}
M_n /= (POSE_T)K;
M += M_n;
JacobiSVD<Matrix<POSE_T, 3,3> > svd(M, ComputeFullU | ComputeFullV);
//[U S V]=svd(M);
//R=U*V';
Matrix<POSE_T, 3, 3> U = svd.matrixU();
Matrix<POSE_T, 3, 3> V = svd.matrixV();
Matrix<POSE_T, 3, 1> D = svd.singularValues();
SO3Group<POSE_T> R_tmp;
if (M.determinant() < 0){
Matrix<POSE_T, 3, 3> I = Matrix<POSE_T, 3, 3>::Identity(); I(2, 2) = -1; D(2) *= -1;
R_tmp = SO3Group<POSE_T>(U*I*V.transpose());
}
else{
R_tmp = SO3Group<POSE_T>(U*V.transpose());
}
//T=ccent'-R*wcent';
Matrix< POSE_T, 3, 1> T_tmp = Cc - R_tmp * Cw;
RT solution = RT( R_tmp, T_tmp) ;
//T tr = D.sum();
//T dE_sqr = sigma_c_sqr - tr*tr / sigma_w_sqr;
//PRINT(dE_sqr);
return solution; // dE_sqr;
}
template< typename POSE_T, typename POINT_T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
void nl_kneip_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, const POINT_T thre_2d_, const POINT_T nl_thre,
int& Iter, POINT_T confidence = 0.99){
typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
typedef Matrix<POINT_T, 3, 1> Point3;
typedef SE3Group<POSE_T> RT;
POSE_T cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
POINT_T cos_nl_thre = cos(nl_thre);
RandomElements<int> re((int)adapter.getNumberCorrespondences());
const int K = 3;
MatrixX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
MatrixX Nw(3, K + 1), Nc(3, K + 1);
Matrix<short, Dynamic, Dynamic> inliers_kneip(adapter.getNumberCorrespondences(), 3);
adapter.setMaxVotes(-1);
for (int ii = 0; ii < Iter; ii++) {
//randomly select K candidates
RT solution_kneip;
vector<int> selected_cols;
re.run(K + 1, &selected_cols);
assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv);
if (!kneip<POSE_T, POINT_T>(Xw, bv, &solution_kneip)) continue;
//collect votes
int votes_kneip = 0;
inliers_kneip.setZero();
Point3 eivE; Point3 pc; POINT_T cos_a;
for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
if (adapter.isValid(c)){
//with normal data
POINT_T cos_alpha = adapter.getNormalCurr(c).dot(solution_kneip.so3().template cast<POINT_T>() * adapter.getNormalGlob(c));
if (cos_alpha > cos_nl_thre){
inliers_kneip(c, 2) = 1;
votes_kneip++;
}
}
//with 2d
pc = solution_kneip.so3().template cast<POINT_T>() * adapter.getPointGlob(c) + solution_kneip.translation().template cast<POINT_T>();// transform pw into pc
pc = pc / pc.norm(); //normalize pc
//compute the score
cos_a = pc.dot( adapter.getBearingVector(c) );
if (cos_a > cos_thr){
inliers_kneip(c, 0) = 1;
votes_kneip++;
}
}
//cout << endl;
if ( votes_kneip > adapter.getMaxVotes() ){
assert(votes_kneip == inliers_kneip.sum());
adapter.setMaxVotes(votes_kneip);
adapter.setRcw(solution_kneip.so3());
adapter.sett(solution_kneip.translation());
adapter.setInlier(inliers_kneip);
//cout << inliers_kneip.inverse() << endl << endl;
//adapter.printInlier();
//Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 2 - votes_kneip) / adapter.getNumberCorrespondences() / 2, K, Iter);
}
}
PnPPoseAdapter<POSE_T, POINT_T>* pAdapter = &adapter;
pAdapter->cvtInlier();
adapter.cvtInlier();
return;
}
template< typename POSE_T, typename POINT_T > /*Eigen::Matrix<float,-1,-1,0,-1,-1> = Eigen::MatrixXf*/
void nl_shinji_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter, const POINT_T thre_3d_, const POINT_T nl_thre,
int& Iter, POINT_T confidence = 0.99){
// eimXc_ = R * eimXw_ + T; //R and t is defined in world and transform a point in world to local
typedef Matrix<POINT_T, Dynamic, Dynamic> MatrixX;
typedef Matrix<POINT_T, 3, 1> Point3;
typedef SE3Group<POSE_T> RT;
POINT_T cos_nl_thre = cos(nl_thre);
RandomElements<int> re((int)adapter.getNumberCorrespondences());
const int K = 3;
MatrixX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
MatrixX Nw(3, K + 1), Nc(3, K + 1);
Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);
adapter.setMaxVotes(-1);
for (int ii = 0; ii < Iter; ii++) {
//randomly select K candidates
RT solution_shinji, solution_nl;
vector<int> selected_cols;
re.run(K + 1, &selected_cols);
vector<RT> v_solutions;
if (assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
solution_shinji = shinji<POSE_T, POINT_T>(Xw, Xc, K);
v_solutions.push_back(solution_shinji);
}
nl_2p<POSE_T, POINT_T>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
v_solutions.push_back(solution_nl);
for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
//collect votes
int votes = 0;
inliers.setZero();
Point3 eivE; Point3 pc; POINT_T score;
for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
if (adapter.isValid(c)){
//voted by N-N correspondences
POINT_T cos_alpha = adapter.getNormalCurr(c).dot(itr->so3().template cast<POINT_T>() * adapter.getNormalGlob(c));
if (cos_alpha > cos_nl_thre){
inliers(c, 2) = 1;
votes++;
}
//voted by 3-3 correspondences
eivE = adapter.getPointCurr(c) - (itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>());
if (eivE.norm() < thre_3d_){
inliers(c, 1) = 1;
votes++;
}
}
}
//cout << endl;
if (votes > adapter.getMaxVotes() ){
assert(votes == inliers.sum());
adapter.setMaxVotes(votes);
adapter.setRcw(itr->so3());
adapter.sett(itr->translation());
adapter.setInlier(inliers);
//adapter.printInlier();
//Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 2 - votes) / adapter.getNumberCorrespondences() / 2, K, Iter);
}
}
}
AOPoseAdapter<POSE_T, POINT_T>* pAdapter = &adapter;
pAdapter->cvtInlier();
adapter.cvtInlier();
return;
}