// Assume t = double[3], q = double[4]
void EstimateTfSVD(double* t, double* q)
{
// Assemble the correlation matrix H = target * reference'
Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d u = svd.matrixU ();
Eigen::Matrix3d v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int i = 0; i < 3; ++i)
v (i, 2) *= -1;
}
// std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl;
// std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl;
Eigen::Matrix3d R = v * u.transpose ();
const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ());
Eigen::Vector3d T = centroid_ref.head<3> () - Rc;
// Make sure these memory locations are valid
assert(t != NULL && q!=NULL);
Eigen::Quaterniond Q(R);
t[0] = T(0); t[1] = T(1); t[2] = T(2);
q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z();
}
void pose_estimation_3d3d (
const vector<Point3f>& pts1,
const vector<Point3f>& pts2,
Mat& R, Mat& t
)
{
Point3f p1, p2; // center of mass
int N = pts1.size();
for ( int i=0; i<N; i++ )
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f( Vec3f(p1) / N);
p2 = Point3f( Vec3f(p2) / N);
vector<Point3f> q1 ( N ), q2 ( N ); // remove the center
for ( int i=0; i<N; i++ )
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for ( int i=0; i<N; i++ )
{
W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
}
cout<<"W="<<W<<endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
if (U.determinant() * V.determinant() < 0)
{
for (int x = 0; x < 3; ++x)
{
U(x, 2) *= -1;
}
}
cout<<"U="<<U<<endl;
cout<<"V="<<V<<endl;
Eigen::Matrix3d R_ = U* ( V.transpose() );
Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );
// convert to cv::Mat
R = ( Mat_<double> ( 3,3 ) <<
R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
);
t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}
void tangent_and_bitangent(const Eigen::Vector3d & n_,
Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
double rmin = 0.99;
double n0 = n_(0), n1 = n_(1), n2 = n_(2);
if (std::abs(n0) <= rmin) {
rmin = std::abs(n0);
t_(0) = 0.0;
t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
t_(2) = n1 / std::sqrt(1.0 - std::pow(n0, 2));
}
if (std::abs(n1) <= rmin) {
rmin = std::abs(n1);
t_(0) = n2 / std::sqrt(1.0 - std::pow(n1, 2));
t_(1) = 0.0;
t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
}
if (std::abs(n2) <= rmin) {
rmin = std::abs(n2);
t_(0) = n1 / std::sqrt(1.0 - std::pow(n2, 2));
t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
t_(2) = 0.0;
}
b_ = n_.cross(t_);
// Check that the calculated Frenet-Serret frame is left-handed (levogiro)
// by checking that the determinant of the matrix whose columns are the normal,
// tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
Eigen::Matrix3d M;
M.col(0) = n_;
M.col(1) = t_;
M.col(2) = b_;
if (boost::math::sign(M.determinant()) != 1) {
PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
}
}
Gaussian3D get_gaussian_from_covariance(const Eigen::Matrix3d &covar,
const Vector3D ¢er) {
Rotation3D rot;
Vector3D radii;
// get eigen decomposition and sort by eigen vector
Eigen::EigenSolver<Eigen::Matrix3d> es(covar);
Eigen::Matrix3d evecs = es.eigenvectors().real();
Eigen::Vector3d evals = es.eigenvalues().real();
// fill in sorted stuff
for (int i = 0; i < 3; i++) {
radii[i] = evals[i];
}
// reflect if necessary
double det = evecs.determinant();
// std::cout<<"Determinant is "<<det<<std::endl;
if (det < 0) {
Eigen::Matrix3d reflect = Eigen::Vector3d(1, 1, -1).asDiagonal();
evecs = evecs * reflect;
}
// create rotation matrix and return
Eigen::Quaterniond eq(evecs);
rot = Rotation3D(eq.w(), eq.x(), eq.y(), eq.z());
return Gaussian3D(ReferenceFrame3D(Transformation3D(rot, center)), radii);
}
double LinearAlgebra::detOfMatrixM(const Eigen::Matrix3d& M)
{
double result;
// TODO: return the determinant of matrix M
result = M.determinant();
return result;
}
bool checkCoherentRotation(Eigen::Matrix3d &Rot)
{
double det1 = Rot.determinant();
if(fabsf( det1 )-1.0 > 1e-07)
{
std::cerr << "det(R) != +-1.0, this is not a rotation matrix" << '\n';
return false;
}
return true;
}
void MarkerArrayVisualizer::drawGaussianDistributions(const char* ns, const std::vector<Eigen::Vector3d>& mu, const std::vector<Eigen::Matrix3d>& sigma, double probability, const std::vector<double>& offset)
{
visualization_msgs::MarkerArray marker_array;
const int size = mu.size();
for (int i=0; i<size; i++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "/world";
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
// axis: eigenvectors
// radius: eigenvalues
Eigen::JacobiSVD<Eigen::MatrixXd> svd(sigma[i], Eigen::ComputeThinU | Eigen::ComputeThinV);
const Eigen::VectorXd& r = svd.singularValues();
Eigen::Matrix3d Q = svd.matrixU();
// to make determinant 1
if (Q.determinant() < 0)
Q.col(2) *= -1.;
const Eigen::Quaterniond q(Q);
marker.pose.position.x = mu[i](0);
marker.pose.position.y = mu[i](1);
marker.pose.position.z = mu[i](2);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
const double probability_radius = gaussianDistributionRadius3D(probability);
marker.scale.x = 2. * (probability_radius * std::sqrt(r[0]) + offset[i]);
marker.scale.y = 2. * (probability_radius * std::sqrt(r[1]) + offset[i]);
marker.scale.z = 2. * (probability_radius * std::sqrt(r[2]) + offset[i]);
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 0.25;
marker.lifetime = ros::Duration();
marker_array.markers.push_back(marker);
}
publish(marker_array);
}
TEST(OdometryCalibration, Calibration) {
FileIO fileIO;
std::string path = ros::package::getPath("odometry_calibration") + "/data/calib.dat";
try {
fileIO.loadFromFile(path.c_str());
} catch(const std::runtime_error& e) {
ASSERT_TRUE(false) << "Could not load calibration data file";
}
Eigen::Matrix3d calibrationMatrix = OdometryCalibration::calibrateOdometry(fileIO.measurements);
ASSERT_NEAR(-0.0754092, calibrationMatrix.determinant(), 1e-5);
}
void MarkerArrayVisualizer::drawEllipsoids(const char* ns, const std::vector<Eigen::Vector3d>& center, const std::vector<Eigen::Matrix3d>& A)
{
visualization_msgs::MarkerArray marker_array;
const int size = center.size();
for (int i=0; i<size; i++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "/world";
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
// axis: eigenvectors
// radius: eigenvalues
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A[i], Eigen::ComputeThinU | Eigen::ComputeThinV);
const Eigen::VectorXd& r = svd.singularValues();
Eigen::Matrix3d Q = svd.matrixU();
// to make determinant 1
if (Q.determinant() < 0)
Q.col(2) *= -1.;
const Eigen::Quaterniond q(Q);
marker.pose.position.x = center[i](0);
marker.pose.position.y = center[i](1);
marker.pose.position.z = center[i](2);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
marker.scale.x = 2. * r(0);
marker.scale.y = 2. * r(1);
marker.scale.z = 2. * r(2);
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 0.2;
marker.lifetime = ros::Duration();
marker_array.markers.push_back(marker);
}
publish(marker_array);
}
bool determinantCorrection(Eigen::Matrix3d &Rot)
{
double det1 = Rot.determinant();
if( det1 < -0.9999999 && det1 > -1.0000001 )
{
// When a rotation matrix is a reflection because its determinant is -1
Rot.col(2) = -1.0*Rot.col(2); // Corrects the third column sign
std::cout << "Rotation matrix has det = -1, correct procedure to achieve det = 1\n";
return true;
}
return false;
}
double PCGMMReg_func::weight_l2(PCObject &model, PCObject &scene)
{
// reference :
// Robust Point Set Registration Using Gaussian Mixture Models
// Bing Jina, and Baba C. Vemuri
// IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
double energy1 = 0.;
for(int i=0;i<m;i++){
for(int j=0;j<m;j++){
Eigen::Matrix3d cov = model.gmm.at(i).covariance + model.gmm.at(j).covariance;
Eigen::Vector3d mean = model.gmm.at(i).mean - model.gmm.at(j).mean;
Eigen::Matrix3d invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
energy1 += model.gmm.at(i).weight*model.gmm.at(j).weight*gauss;
}
}
// cout<<"m "<<m<<endl;
// cout<<"s "<<s<<endl;
double energy2 = 0.;
for(int i=0;i<m;i++){
double sum[3] = {0.,0.,0.};
for(int j=0;j<s;j++){
Eigen::Matrix3d cov = model.gmm.at(i).covariance + scene.gmm.at(j).covariance;
Eigen::Vector3d mean = model.gmm.at(i).mean - scene.gmm.at(j).mean;
Eigen::Matrix3d invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
energy2 += model.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
// cout<<"weight i "<<model.gmm.at(i).weight<<endl;
// cout<<"weight j "<<scene.gmm.at(j).weight<<endl;
// cout<<"a "<<a<<endl;
// cout<<"gauss "<<gauss<<endl;
// gradient [m,d]
double derv_x = -0.5*(2*mean[0]*invij(0,0) + mean[1]*(invij(0,1)+invij(1,0)) + mean[2]*(invij(0,2)+invij(2,0)));
double derv_y = -0.5*(mean[0]*(invij(1,0)+invij(0,1)) + 2*mean[1]*invij(1,1) + mean[2]*(invij(1,2)+invij(2,1)));
double derv_z = -0.5*(mean[0]*(invij(2,0)+invij(0,2)) + mean[1]*(invij(2,1)+invij(1,2)) + 2*mean[2]*invij(2,2));
sum[0] += scene.gmm.at(j).weight*gauss*derv_x;
sum[1] += scene.gmm.at(j).weight*gauss*derv_y;
sum[2] += scene.gmm.at(j).weight*gauss*derv_z;
}
gradient[i][0] = -2.*model.gmm.at(i).weight*sum[0];
gradient[i][1] = -2.*model.gmm.at(i).weight*sum[1];
gradient[i][2] = -2.*model.gmm.at(i).weight*sum[2];
}
double energy3 = 0.;
for(int i=0;i<s;i++){
for(int j=0;j<s;j++){
Eigen::Matrix3d cov = scene.gmm.at(i).covariance + scene.gmm.at(j).covariance;
Eigen::Vector3d mean = scene.gmm.at(i).mean - scene.gmm.at(j).mean;
Eigen::Matrix3d invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,3)*cov.determinant())*exp(-0.5*a);
energy3 += scene.gmm.at(i).weight*scene.gmm.at(j).weight*gauss;
}
}
return energy1 - 2*energy2 + energy3;
// cout<<"energy2 "<<energy2<<endl;
// return -2*energy2;
}
double covToSDMetres(const Eigen::Matrix3d & S)
{
return pow(S.determinant(), 1.0/6.0); //Approx. sigma in metres
}
void mesh_core::findSphereTouching4Points(
Eigen::Vector3d& center,
double& radius,
const Eigen::Vector3d& a,
const Eigen::Vector3d& b,
const Eigen::Vector3d& c,
const Eigen::Vector3d& d)
{
Eigen::Matrix3d m;
m.col(0) = b - a;
m.col(1) = c - a;
m.col(2) = d - a;
double det = m.determinant();
// points are coplanar?
if (std::abs(det) <= std::numeric_limits<double>::epsilon())
{
findSphereTouching4PointsCoplanar(center, radius, a,b,c,d);
if (g_verbose)
{
logInform("findSphereTouching4Points() COPLANAR QQQQ");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" d = (%7.3f %7.3f %7.3f)",
d.x(),
d.y(),
d.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
return;
}
double ab_lensq = m.col(0).squaredNorm();
double ac_lensq = m.col(1).squaredNorm();
double ad_lensq = m.col(2).squaredNorm();
Eigen::Vector3d rel_center = ((ad_lensq * m.col(0).cross(m.col(1))) +
(ac_lensq * m.col(2).cross(m.col(0))) +
(ab_lensq * m.col(1).cross(m.col(2)))) /
(2.0 * det);
radius = rel_center.norm();
center = rel_center + a;
if (g_verbose)
{
logInform("findSphereTouching4Points()");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" d = (%7.3f %7.3f %7.3f)",
d.x(),
d.y(),
d.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
}
/*
* Compute the homography parameters
*/
void RSTEstimator::leastSquaresEstimate(std::vector<std::pair<Eigen::Vector3d,Eigen::Vector3d> *> &points,
std::vector<double> ¶meters)
{
unsigned int N = points.size();
//checks
if (N<3) {
std::cout << "At least 3 point correspondences are needed" << std::endl;
return;
}
Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
Eigen::MatrixXd model(3,N);
Eigen::MatrixXd data(3,N);
Eigen::MatrixXd Ryx;
Eigen::Matrix3d R;
Eigen::Vector3d T;
//compute centroids
for (int i=0 ; i<N; i++) {
model.block(0,i,3,1) = points[i]->first;
data.block(0,i,3,1) = points[i]->second;
centroide_model += points[i]->first;
centroide_data += points[i]->second;
}
centroide_model = centroide_model/N;
centroide_data = centroide_data/N;
//Subtract centroids to data
for (int i=0; i<N; i++){
model.block(0,i,3,1) -= centroide_model;
data.block(0,i,3,1) -= centroide_data;
}
Ryx = data * model.transpose(); //maps data into model
JacobiSVD<Eigen::Matrix3d> svd(Ryx, ComputeFullU | ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
if (U.determinant()*V.determinant()<0) {
for (int i=0; i<3; ++i) {
V(i,2) *=-1;
}
}
R = V * U.transpose();
T = centroide_model - R*centroide_data;
parameters.push_back(R(0,0)); //R11
parameters.push_back(R(0,1)); //R12
parameters.push_back(R(0,2)); //R13
parameters.push_back(R(1,0)); //R21
parameters.push_back(R(1,1)); //R22
parameters.push_back(R(1,2)); //R23
parameters.push_back(R(2,0)); //R31
parameters.push_back(R(2,1)); //R32
parameters.push_back(R(2,2)); //R33
parameters.push_back(T(0)); //T1
parameters.push_back(T(1)); //T2
parameters.push_back(T(2)); //T3
/*
H = | Theta1 Theta2 Theta3 Theta10 |
| Theta4 Theta5 Theta6 Theta11 |
| Theta7 Theta8 Theta9 Theta12 |
| 0 0 0 1 |
*/
}
//procustres
inline void ICP::minimize(const Pair& init_f){
Eigen::Vector3d centroide_model(0.0,0.0,0.0), centroide_data(0.0,0.0,0.0);
Eigen::Matrix3d M;
unsigned int N = data_indices.size() + init_f.size();
Eigen::MatrixXd model(3,N);
Eigen::MatrixXd data(3,N);
//calcula os centroides
int k=0;
for(unsigned int i=0; i<N; i++){
if (i<data_indices.size()){
model(0,i) = cloud_m->points[ model_indices[i] ].x;
model(1,i) = cloud_m->points[ model_indices[i] ].y;
model(2,i) = cloud_m->points[ model_indices[i] ].z;
data(0,i) = cloud_d->points[ data_indices[i] ].x*T(0,0) + cloud_d->points[ data_indices[i] ].y*T(0,1) + cloud_d->points[ data_indices[i] ].z*T(0,2) + T(0,3);
data(1,i) = cloud_d->points[ data_indices[i] ].x*T(1,0) + cloud_d->points[ data_indices[i] ].y*T(1,1) + cloud_d->points[ data_indices[i] ].z*T(1,2) + T(1,3);
data(2,i) = cloud_d->points[ data_indices[i] ].x*T(2,0) + cloud_d->points[ data_indices[i] ].y*T(2,1) + cloud_d->points[ data_indices[i] ].z*T(2,2) + T(2,3);
}else{
model(0,i) = cloud_m->points[ init_f[k].first ].x;
model(1,i) = cloud_m->points[ init_f[k].first ].y;
model(2,i) = cloud_m->points[ init_f[k].first ].z;
data(0,i) = cloud_d->points[ init_f[k].second ].x*T(0,0) + cloud_d->points[ init_f[k].second ].y*T(0,1) + cloud_d->points[ init_f[k].second ].z*T(0,2) + T(0,3);
data(1,i) = cloud_d->points[ init_f[k].second ].x*T(1,0) + cloud_d->points[ init_f[k].second ].y*T(1,1) + cloud_d->points[ init_f[k].second ].z*T(1,2) + T(1,3);
data(2,i) = cloud_d->points[ init_f[k].second ].x*T(2,0) + cloud_d->points[ init_f[k].second ].y*T(2,1) + cloud_d->points[ init_f[k].second ].z*T(2,2) + T(2,3);
k++;
}
centroide_model += model.block(0,i,3,1);
centroide_data += data.block(0,i,3,1);
}
centroide_data = centroide_data/N;
centroide_model = centroide_model/N;
//subtrai os centroides aos dados
for (unsigned int i=0; i<N; i++){
model.block(0,i,3,1) -= centroide_model;
data.block(0,i,3,1) -= centroide_data;
}
//Determina a transformacao
M = data*model.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
if (U.determinant()*V.determinant()<0) {
for (int i=0; i<3; ++i)
V(i,2) *=-1;
}
Eigen::Matrix3d r = V * U.transpose();
Eigen::Vector3d t = centroide_model - r * centroide_data;
//~ T.block<3,3>(0,0) = r*T.block<3,3>(0,0);
//~ T.block<3,1>(0,3) += t;
T.block<3,1>(0,3) = T.block<3,3>(0,0)*t + T.block<3,1>(0,3);
T.block<3,3>(0,0) = T.block<3,3>(0,0)*r;
}
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
const std::vector<cv::DMatch> &matches)
{
// convert keypoints in match to 3d points
std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;
int nmatch = matches.size();
//srand(getDoubleTime());
// set up data structures for fast processing
// indices to good matches
std::vector<int> m0, m1;
for (int i=0; i<nmatch; i++)
{
m0.push_back(matches[i].queryIdx);
m1.push_back(matches[i].trainIdx);
//std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
}
nmatch = m0.size();
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
// RANSAC loop
//#pragma omp parallel for shared( bestinl )
for (int i=0; i<numRansac; i++)
{
//std::cout << "ransac loop : " << i << std::endl;
// find a candidate
int a=rand()%nmatch;
int b = a;
while (a==b)
b=rand()%nmatch;
int c = a;
while (a==c || b==c)
c=rand()%nmatch;
int i0a = m0[a];
int i0b = m0[b];
int i0c = m0[c];
int i1a = m1[a];
int i1b = m1[b];
int i1c = m1[c];
if (i0a == i0b || i0a == i0c || i0b == i0c ||
i1a == i1b || i1a == i1c || i1b == i1c)
continue;
//std::cout<<a<<" "<<b<<" "<<c<<std::endl;
//std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
//std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;
// get centroids
Eigen::Vector3d p0a = f0.pts[i0a].head(3);
Eigen::Vector3d p0b = f0.pts[i0b].head(3);
Eigen::Vector3d p0c = f0.pts[i0c].head(3);
Eigen::Vector3d p1a = f1.pts[i1a].head(3);
Eigen::Vector3d p1b = f1.pts[i1b].head(3);
Eigen::Vector3d p1c = f1.pts[i1c].head(3);
Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);
//std::cout<<c0.transpose()<<std::endl;
//std::cout<<c1.transpose()<<std::endl;
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
// do the SVD thang
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Eigen::Vector3d tr = c0-R*c1; // translation
// transformation matrix, 3x4
Eigen::Matrix<double,3,4> tfm;
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.block<3,3>(0,0) = R;
tfm.col(3) = tr;
#if 0
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Vector3d pt = tfm*f1.pts[m1[i]];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[m0[i]];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[m0[i]] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
// inl+=(int)sqrt(ipt.z()/matches[i].distance);
// cout << "matches[i].distance : " << matches[i].distance << endl;
// inl++;
}
}
#endif
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);
// double z = fabs(pt1.z() - pt0.z())*0.5;
double z = pt1.z();
double dx = pt1.x() - pt0.x();
double dy = pt1.y() - pt0.y();
double dd = pt1.z() - pt0.z();
if (projectMatches)
{
// The central idea here is to divide by the distance (this is essentially what cam2pix does).
dx = dx / z;
dy = dy / z;
}
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
//---- inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
// inl+=(int)sqrt(ipt.z()/matches[i].distance);
// cout << "matches[i].distance : " << matches[i].distance << endl;
inl++;
}
}
//#pragma omp critical
if (inl > bestinl)
{
bestinl = inl;
rot = R;
trans = tr;
// std::cout << "bestinl : " << bestinl << std::endl;
}
}
//printf("Best inliers: %d\n", bestinl);
//printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
std::vector<cv::DMatch> inls; // temporary for current inliers
inliers.clear();
Eigen::Matrix<double,3,4> tfm;
tfm.block<3,3>(0,0) = rot;
tfm.col(3) = trans;
//std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
//std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;
nmatch = matches.size();
for (int i=0; i<nmatch; i++)
{
Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
//Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
//Vector3d pt1 = pt1_unchanged;
#if 0
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[matches[i].queryIdx] - ipt.z();
#endif
Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);
//double z = fabs(pt1.z() - pt0.z())*0.5;
double z = pt1.z();
double dx = pt1.x() - pt0.x();
double dy = pt1.y() - pt0.y();
double dd = pt1.z() - pt0.z();
if (projectMatches)
{
// The central idea here is to divide by the distance (this is essentially what cam2pix does).
dx = dx / z;
dy = dy / z;
}
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
if (z < maxDist && z > minDist)
// if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
{
// std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
// f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
inliers.push_back(matches[i]);
}
}
}
#if 0
// Test with the SBA...
{
// system
SysSBA sba;
sba.verbose = 0;
#if 0
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = f0.pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0); // 2.0
cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
quat = q1;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
}
#endif
// std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
// std::cout << rot << std::endl;
return inliers.size();
}