void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
Eigen::VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
std::vector<int> fillSubset(const Eigen::MatrixXf &data, Eigen::MatrixXf &subset, int num_columns)
{
std::vector<int> column_indices;
for(int i = 0; i < num_columns; i++)
{
int column_number = rand() % data.cols();
column_indices.push_back(column_number);
subset.col(i) = data.col(column_number);
//subset.col(i) = data.col(100 + i);
}
return column_indices;
}
double RMSEcalc(std::vector< std::vector<int> > traindata, Eigen::MatrixXf U, Eigen::MatrixXf M) {
Eigen::VectorXf sqvals(traindata[1].size());
#pragma omp parallel for
for(int p=0;p<traindata[1].size();p++) {
sqvals(p)=(U.col(traindata[0][p]-1).transpose()*M.col(traindata[1][p]-1) - traindata[2][p])*(U.col(traindata[0][p]-1).transpose()*M.col(traindata[1][p]-1) - traindata[2][p]);
}
double total;
total = sqvals.sum();
return total;
}
void calcMeanAndCovarWeighedVectorized(const Eigen::MatrixXf &input, const Eigen::VectorXd &inputWeights, Eigen::MatrixXf &out_covMat, Eigen::VectorXf &out_mean,Eigen::MatrixXf &temp)
{
out_mean=input.col(0); //to resize
out_mean.setZero();
double wSumInv=1.0/inputWeights.sum();
for (int k=0;k<inputWeights.size();k++){
double w=inputWeights[k];
out_mean+=input.col(k)*(float)(w*wSumInv);
}
out_mean = input.rowwise().mean();
temp = (input.colwise() - out_mean);
for (int k=0;k<inputWeights.size();k++){
temp.col(k) *= (float)(sqrt(inputWeights(k)*wSumInv)); //using square roots, as we only want the normalized weights to be included once for each result element in the multiplication below
}
out_covMat = temp*temp.transpose();
}
void coordinateDescentLasso(
const Eigen::MatrixXf &data,
const Eigen::VectorXf &output,
Eigen::VectorXf &weights,
const float lambda,
const int nIters,
const bool verbose)
{
const int nExamples = data.rows();
const int nFeatures = data.cols();
for(int iter = 0; iter < nIters; ++iter){
const int featureInd = iter % nFeatures;
float rho = 0;
for(int i = 0; i < nExamples; ++i)
rho += residualWithoutKWeight(
weights,
data.row(i).transpose(),
output[i],
featureInd) * data(i, featureInd);
auto column = data.col(featureInd);
float sumOfColumn = (column.transpose() * column).sum();
weights[featureInd] = coordinateDescentStepLasso(weights[featureInd], sumOfColumn, rho, lambda);
if(verbose){
const float err = rss(weights, data, output);
std::cout << "iter " << iter << " err " << err << std::endl;
std::cout << weights << std::endl;
}
}
}
VectorXs DenseCRF::currentMap( const Eigen::MatrixXf & Q ) const{
VectorXs r(Q.cols());
// Find the map
for( int i=0; i<N_; i++ ){
int m;
Q.col(i).maxCoeff( &m );
r[i] = m;
}
return r;
}
float
computeHistogramIntersection (const Eigen::VectorXf &histA, const Eigen::VectorXf &histB)
{
Eigen::MatrixXf histAB (histA.rows(), 2);
histAB.col(0) = histA;
histAB.col(1) = histB;
Eigen::VectorXf minv = histAB.rowwise().minCoeff();
return minv.sum();
}
void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
// Update the position of the first vertex if the keys 1,2, or 3 are pressed
switch (key)
{
case GLFW_KEY_1:
V.col(0) << -0.5, 0.5;
break;
case GLFW_KEY_2:
V.col(0) << 0, 0.5;
break;
case GLFW_KEY_3:
V.col(0) << 0.5, 0.5;
break;
default:
break;
}
// Upload the change to the GPU
VBO.update(V);
}
Eigen::Vector4f PhotoCamera::center(const Eigen::MatrixXf &camera)
{
Eigen::Matrix3f C1, C2, C3, C4;
C1 << camera.col(1), camera.col(2), camera.col(3);
C2 << camera.col(0), camera.col(2), camera.col(3);
C3 << camera.col(0), camera.col(1), camera.col(3);
C4 << camera.col(0), camera.col(1), camera.col(2);
Eigen::Vector4f C;
C << C1.determinant(), -C2.determinant(), C3.determinant(), -C4.determinant();
return C;
}
void HierarchicalWalkingIK::computeWalkingTrajectory(const Eigen::Matrix3Xf& comTrajectory,
const Eigen::Matrix6Xf& rightFootTrajectory,
const Eigen::Matrix6Xf& leftFootTrajectory,
std::vector<Eigen::Matrix3f>& rootOrientation,
Eigen::MatrixXf& trajectory)
{
int rows = outputNodeSet->getSize();
trajectory.resize(rows, rightFootTrajectory.cols());
rootOrientation.resize(rightFootTrajectory.cols());
Eigen::Vector3f com = colModelNodeSet->getCoM();
Eigen::VectorXf configuration;
int N = trajectory.cols();
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f rightFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f chestPose = chest->getGlobalPose();
Eigen::Matrix4f pelvisPose = pelvis->getGlobalPose();
for (int i = 0; i < N; i++)
{
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1), leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
VirtualRobot::MathTools::posrpy2eigen4f(1000 * rightFootTrajectory.block(0, i, 3, 1), rightFootTrajectory.block(3, i, 3, 1), rightFootPose);
// FIXME the orientation of the chest and chest is specific to armar 4
// since the x-Axsis points in walking direction
Eigen::Vector3f xAxisChest = (leftFootPose.block(0, 1, 3, 1) + rightFootPose.block(0, 1, 3, 1))/2;
xAxisChest.normalize();
chestPose.block(0, 0, 3, 3) = Bipedal::poseFromXAxis(xAxisChest);
pelvisPose.block(0, 0, 3, 3) = Bipedal::poseFromYAxis(-xAxisChest);
std::cout << "Frame #" << i << ", ";
robot->setGlobalPose(leftFootPose);
computeStepConfiguration(1000 * comTrajectory.col(i),
rightFootPose,
chestPose,
pelvisPose,
configuration);
trajectory.col(i) = configuration;
rootOrientation[i] = leftFootPose.block(0, 0, 3, 3);
}
}
Eigen::VectorXf ZMeshFilterManifold::computeMaxEigenVector(const Eigen::MatrixXf& inputMat, const std::vector<bool>& clusterK)
{
//Eigen::VectorXf ret(rangeDim_);
Eigen::VectorXf ret(3);
ret.fill(0);
int count = 0;
for (int i=0; i<clusterK.size(); i++) if (clusterK[i]) count++;
Eigen::MatrixXf realInput(count, rangeDim_);
//Eigen::MatrixXf realInput(count, 3);
count = 0;
for (int i=0; i<clusterK.size(); i++)
{
if (clusterK[i])
{
realInput.row(count) = inputMat.row(i);
//realInput.row(count) = MATH::spherical2Cartesian(inputMat.row(i));
count++;
}
}
Eigen::MatrixXf mat = realInput.transpose()*realInput;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenSolver(mat);
if (eigenSolver.info()!=Eigen::Success) {
std::cerr << "Error in SVD decomposition!\n";
return ret;
}
Eigen::VectorXf eigenValues = eigenSolver.eigenvalues();
Eigen::MatrixXf eigenVectors = eigenSolver.eigenvectors();
int maxIdx = 0;
float maxValue = eigenValues(maxIdx);
for (int i=1; i<eigenValues.size(); i++)
{
if (eigenValues(i)>maxValue)
{
maxIdx = i;
maxValue = eigenValues(maxIdx);
}
}
ret = eigenVectors.col(maxIdx);
return ret;
// Eigen::VectorXf retSph = MATH::cartesian2Spherical(ret, 1);
// return retSph.head(rangeDim_);
}
void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
{
// Get the position of the mouse in the window
double xpos, ypos;
glfwGetCursorPos(window, &xpos, &ypos);
// Get the size of the window
int width, height;
glfwGetWindowSize(window, &width, &height);
// Convert screen position to world coordinates
double xworld = ((xpos/double(width))*2)-1;
double yworld = (((height-1-ypos)/double(height))*2)-1; // NOTE: y axis is flipped in glfw
// Update the position of the first vertex if the left button is pressed
if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS)
V.col(0) << xworld, yworld;
// Upload the change to the GPU
VBO.update(V);
}
void extractControlFrames(VirtualRobot::RobotPtr robot,
const Eigen::Matrix6Xf& leftFootTrajectory,
const Eigen::MatrixXf& bodyTrajectory,
VirtualRobot::RobotNodeSetPtr bodyJoints,
VirtualRobot::RobotNodePtr node,
std::vector<Eigen::Matrix4f>& controlFrames)
{
int N = leftFootTrajectory.cols();
for (int i = 0; i < N; i++)
{
// Move basis along with the left foot
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
robot->setGlobalPose(leftFootPose);
bodyJoints->setJointValues(bodyTrajectory.col(i));
controlFrames.push_back(node->getGlobalPose());
}
}
int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
{
// This problem is R^2 -> R^(fvec.rows()), so the dimension of the Jacobian is fvec.rows() x 2
float epsilon = 1e-5;
Eigen::VectorXf epsilonVector(2);
Eigen::VectorXf f(this->Points.size());
operator()(epsilonVector, f);
for(unsigned int parameter = 0; parameter < 2; parameter++)
{
epsilonVector = x;
epsilonVector(parameter) += epsilon;
Eigen::VectorXf fForward(2);
operator()(epsilonVector, fForward);
fjac.col(parameter) = (fForward - f)/epsilon;
}
return 0;
}
void transformOrientationToGroundFrame(const VirtualRobot::RobotPtr& robot,
const Eigen::Matrix6Xf& leftFootTrajectory,
const VirtualRobot::RobotNodePtr& leftFoot,
const VirtualRobot::RobotNodePtr& rightFoot,
const VirtualRobot::RobotNodeSetPtr& bodyJoints,
const Eigen::MatrixXf& bodyTrajectory,
const Eigen::Matrix3Xf& trajectory,
const std::vector<SupportInterval>& intervals,
Eigen::Matrix3Xf& relativeTrajectory)
{
Eigen::Matrix4f leftInitialPose = bodyJoints->getKinematicRoot()->getGlobalPose();
int N = trajectory.cols();
int M = trajectory.rows();
relativeTrajectory.resize(M, N);
BOOST_ASSERT(M > 0 && M <= 3);
auto intervalIter = intervals.begin();
for (int i = 0; i < N; i++)
{
while (i >= intervalIter->endIdx)
{
intervalIter = std::next(intervalIter);
}
// Move basis along with the left foot
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
robot->setGlobalPose(leftFootPose);
bodyJoints->setJointValues(bodyTrajectory.col(i));
Eigen::Matrix3f worldToRef = computeGroundFrame(leftFoot->getGlobalPose(),
rightFoot->getGlobalPose(),
intervalIter->phase).block(0, 0, 3, 3);
relativeTrajectory.block(0, i, M, 1) = worldToRef.colPivHouseholderQr().solve(trajectory.col(i)).block(0, 0, M, 1);
}
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
bool TraceStoreCol::PcaLossyCompressChunk(int row_start, int row_end,
int col_start, int col_end,
int num_rows, int num_cols, int num_frames,
int frame_stride,
int flow_ix, int flow_frame_stride,
short *data, bool replace,
float *ssq, char *filters,
int row_step, int col_step,
int num_pca) {
Eigen::MatrixXf Ysub, Y, S, Basis;
int loc_num_wells = (col_end - col_start) * (row_end - row_start);
int loc_num_cols = col_end - col_start;
// Take a sample of the data at rate step, avoiding flagged wells
// Count the good rows
int sample_wells = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
char *filt_start = filters + row_ix * num_cols + col_start;
char *filt_end = filt_start + loc_num_cols;
while (filt_start < filt_end) {
if (*filt_start == 0) {
sample_wells++;
}
filt_start += col_step;
}
}
// try backing off to every well rather than just sampled if we didn't get enough
if (sample_wells < MIN_SAMPLE_WELL) {
row_step = 1;
col_step = 1;
int sample_wells = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
char *filt_start = filters + row_ix * num_cols + col_start;
char *filt_end = filt_start + loc_num_cols;
while (filt_start < filt_end) {
if (*filt_start == 0) {
sample_wells++;
}
filt_start += col_step;
}
}
}
if (sample_wells < MIN_SAMPLE_WELL) {
return false; // just give up
}
// Got enough data to work with, zero out the ssq array for accumulation
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
float *ssq_start = ssq + row_ix * num_cols + col_start;
float *ssq_end = ssq_start + loc_num_cols;
while (ssq_start != ssq_end) {
*ssq_start++ = 0;
}
}
// Copy the sampled data in Matrix, frame major
Ysub.resize(sample_wells, num_frames);
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
int sample_offset = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+=row_step) {
size_t store_offset = row_ix * num_cols + col_start;
char *filt_start = filters + store_offset;
char *filt_end = filt_start + loc_num_cols;
int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
float *ysub_start = Ysub.data() + sample_wells * frame_ix + sample_offset;
while (filt_start < filt_end) {
if (*filt_start == 0) {
*ysub_start = *trace_start;
ysub_start++;
sample_offset++;
}
trace_start += col_step;
filt_start += col_step;
}
}
}
// Copy in all the data into working matrix
Y.resize(loc_num_wells, (int)num_frames);
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
int16_t *trace_end = trace_start + loc_num_cols;
float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols;
while( trace_start != trace_end ) {
*y_start++ = *trace_start++;
}
}
}
Eigen::VectorXf col_mean = Y.colwise().sum();
col_mean /= Y.rows();
for (int i = 0; i < Y.cols(); i++) {
Y.col(i).array() -= col_mean.coeff(i);
}
// Create scatter matrix
S = Ysub.transpose() * Ysub;
// Compute the eigenvectors
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
es.compute(S);
Eigen::MatrixXf Pca_Basis = es.eigenvectors();
Eigen::VectorXf Pca_Values = es.eigenvalues();
// Copy top eigen vectors into basis for projection
Basis.resize(num_frames, num_pca);
for (int i = 0; i < Basis.cols(); i++) {
// Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1);
Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
}
// Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast
Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
// Get coefficients to solve
Eigen::MatrixXf B = Y * SX.transpose();
// Uncompress data into yhat matrix
Eigen::MatrixXf Yhat = B * Basis.transpose();
for (int i = 0; i < Yhat.cols(); i++) {
Yhat.col(i).array() += col_mean.coeff(i);
Y.col(i).array() += col_mean.coeff(i);
}
// H5File h5("pca_lossy.h5");
// h5.Open();
// char buff[256];
// snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Y);
// snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Yhat);
// snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Basis);
// h5.Close();
// Copy data out of yhat matrix into original data structure, keeping track of residuals
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
int16_t *trace_start = data + flow_frame_stride * frame_ix + flow_ix * frame_stride + store_offset;
int16_t *trace_end = trace_start + loc_num_cols;
float * ssq_start = ssq + store_offset;
size_t loc_offset = (row_ix - row_start) * loc_num_cols;
float * y_start = Y.data() + loc_num_wells * frame_ix + loc_offset;
float * yhat_start = Yhat.data() + loc_num_wells * frame_ix + loc_offset;
while( trace_start != trace_end ) {
if (replace) {
*trace_start = (int16_t)(*yhat_start + .5);
}
float val = *y_start - *yhat_start;
*ssq_start += val * val;
y_start++;
yhat_start++;
trace_start++;
ssq_start++;
}
}
}
// divide ssq data out for per frame avg
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
float * ssq_start = ssq + store_offset;
float * ssq_end = ssq_start + loc_num_cols;
while (ssq_start != ssq_end) {
*ssq_start /= num_frames;
ssq_start++;
}
}
return true;
}
/**
* @function calculateTrifocalTensor
*/
void trifocalTensor::calculateTrifocalTensor() {
Eigen::JacobiSVD<Eigen::MatrixXf> svd( mEq, Eigen::ComputeThinU | Eigen::ComputeThinV );
Eigen::MatrixXf V = svd.matrixV();
printf("* V has %d rows and %d cols \n", V.rows(), V.cols() );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* Rank of mEq is: %d \n", lu.rank() );
//std::cout << "Columns are nullspace : " << std::endl;
//std::cout<< lu.kernel() << std::endl;
//Eigen::MatrixXf kernel = lu.kernel();
//mmEq(mPointer, ToIndex1(3,1,2)=;3 = kernel.col( kernel.cols() - 1 );
// Eigen::MatrixXf Vt = V.transpose(); mmEq(mPointer, ToIndex1(3,1,2)=;3 = Vt.col( Vt.cols() - 1 );
mT123 = V.col( V.cols() - 1 );
printf("mT123: Rows: %d cols: %d \n", mT123.rows(), mT123.cols() );
// Saving them properly
mT.resize(0);
Eigen::MatrixXf T1(3,3);
T1(0,0) = mT123(0,0); T1(0,1) = mT123(1,0); T1(0,2) = mT123(2,0);
T1(1,0) = mT123(3,0); T1(1,1) = mT123(4,0); T1(1,2) = mT123(5,0);
T1(2,0) = mT123(6,0); T1(2,1) = mT123(7,0); T1(2,2) = mT123(8,0);
mT.push_back(T1);
printf("Saved T1 \n");
Eigen::MatrixXf T2(3,3);
T2(0,0) = mT123(9,0); T2(0,1) = mT123(10,0); T2(0,2) = mT123(11,0);
T2(1,0) = mT123(12,0); T2(1,1) = mT123(13,0); T2(1,2) = mT123(14,0);
T2(2,0) = mT123(15,0); T2(2,1) = mT123(16,0); T2(2,2) = mT123(17,0);
mT.push_back(T2);
printf("Saved T2 \n");
Eigen::MatrixXf T3(3,3);
T3(0,0) = mT123(18,0); T3(0,1) = mT123(19,0); T3(0,2) = mT123(20,0);
T3(1,0) = mT123(21,0); T3(1,1) = mT123(22,0); T3(1,2) = mT123(23,0);
T3(2,0) = mT123(24,0); T3(2,1) = mT123(25,0); T3(2,2) = mT123(26,0);
mT.push_back(T3);
printf("Saved T3 \n");
// Checking
Eigen::MatrixXf res = mEq*mT123;
std::cout << "Checking mEq*T: \n"<< res.transpose() << std::endl;
// Making it with last guy = 1
// Normalizing
for( int i = 0; i < mT.size(); ++i ) {
float temp = mT[i](2,2);
for( int j = 0; j < 3; ++j ) {
for( int k = 0; k < 3; ++k ) {
float orig = mT[i](j,k);
mT[i](j,k) = orig / temp;
}
}
}
// Visualize
for( int i = 0; i < mT.size(); ++i ) {
std::cout << "T("<<i<<"): \n" << mT[i] << std::endl;
}
// Test lines
for( int i = 0; i < mLLL.size(); ++i ) {
Eigen::VectorXf A(3);
Eigen::VectorXf B(3);
Eigen::VectorXf C(3);
Eigen::VectorXf Ap(3);
A(0) = mLLL[i][0].x;
A(1) = mLLL[i][0].y;
A(2) = mLLL[i][0].z;
B(0) = mLLL[i][1].x;
B(1) = mLLL[i][1].y;
B(2) = mLLL[i][1].z;
C(0) = mLLL[i][2].x;
C(1) = mLLL[i][2].y;
C(2) = mLLL[i][2].z;
Eigen::MatrixXf r0, r1, r2;
Eigen::MatrixXf Tt;
Tt = mT[0];
r0 = ( B.transpose() )*Tt*C;
Ap(0) = r0(0,0);
Tt = mT[1];
r1 = ( B.transpose() )*Tt*C;
Ap(1) = r1(0,0);
Tt = mT[2];
r2 = ( B.transpose() )*Tt*C;
Ap(2) = r2(0,0);
// Normalize Ap
float temp = A(2) / Ap(2);
float num;
num = Ap(0)*temp; Ap(0) = num;
num = Ap(1)*temp; Ap(1) = num;
num = Ap(2)*temp; Ap(2) = num;
std::cout <<" ("<<i<<") " <<" A: " << A.transpose() << std::endl;
std::cout <<" ("<<i<<") " <<" Ap: " << Ap.transpose() << std::endl;
}
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
inline
void cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw,
int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2)
{
//A
Eigen::MatrixXf X = XXw;
//B
Eigen::MatrixXf Y = XXc;
Eigen::MatrixXf eyen(n,n);
eyen = Eigen::MatrixXf::Identity(n,n);
Eigen::MatrixXf ones(n,n);
ones.setOnes();
Eigen::MatrixXf K(n,n);
K = eyen - ones/n;
vfloat3 ux;
for(int i =0; i < n; i++)
{
ux = ux + X.col(i);
}
ux = ux/n;
vfloat3 uy;
for(int i =0; i < n; i++)
{
uy = uy + Y.col(i);
}
uy = uy/n;
Eigen::MatrixXf XK(3,n);
XK = X*K;
Eigen::MatrixXf XKarre(3,n);
for(int i = 0 ; i < n ; i++)
{
XKarre(0,i) = XK(0,i)*XK(0,i);
XKarre(1,i) = XK(1,i)*XK(1,i);
XKarre(2,i) = XK(2,i)*XK(2,i);
}
Eigen::VectorXf sumXKarre(n);
float sigmx2 = 0;
for(int i = 0 ; i < n ; i++)
{
sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i);
sigmx2 += sumXKarre[i];
}
sigmx2 /=n;
Eigen::MatrixXf SXY(3,3);
SXY = Y*K*(X.transpose())/n;
JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV);
Eigen::MatrixXf S(3,3);
S = Eigen::MatrixXf::Identity(3,3);
if(SXY.determinant() < 0)
{
S(3,3) = -1;
}
R2 = svd.matrixU() * S * (svd.matrixV()).transpose();
Eigen::MatrixXf D(3,3);
D.setZero();
for(int i = 0 ; i < svd.singularValues().size() ; i++)
{
D(i,i) = (svd.singularValues())[i];
}
float c2 = (D*S).trace()/sigmx2;
t2 = uy - c2*R2*ux;
vfloat3 Xx = R2.col(0);
vfloat3 Yy = R2.col(1);
vfloat3 Zz = R2.col(2);
if((x_cross(Xx,Yy)-Zz).norm()>2e-2)
{
R2.col(2) = -Zz;
}
}
Eigen::Matrix<double,6,1> rt_dlt(const Eigen::MatrixX3f & obj_pts,const Eigen::MatrixX3f & proj_pts)
{
//Check the number of points that we have available as inputs
//Demand that there be atleast 6 points input.
if(obj_pts.rows()<6)
{
std::cout << "Need a minimum of 6 points\n";
}
if(obj_pts.rows()!=proj_pts.rows())
{
std::cout << "Need and equal nummber of Object Points and Normalized Projection Points\n";
}
//To ensure numerical stability, we expect the model coordinates to be well behaved as well
//(Small values, close to 1)
//Create a vector of size 12, that would hold the rotation and translation elements, estimate
//it using DLT (SVD and other magic)
Eigen::VectorXf v(12);
Eigen::MatrixXf M(2*obj_pts.rows(),12);
//
//See http://www.maths.lth.se/matematiklth/personal/calle/datorseende13/notes/forelas3.pdf for
//details
//Populate M
for(int i=0; i< obj_pts.rows(); i++)
{
//Eigen::RowVectorXf & Xrow(12);
//Xrow = M.row(2*i);
//Eigen::RowVectorXf & Yrow(12);
//Yrow= M.row(2*i+1);
Eigen::Vector3f ob_pt = obj_pts.row(i);
Eigen::Vector3f pr_pt = proj_pts.row(i);
M(2*i,0) = ob_pt(0); //-X
M(2*i,1) = ob_pt(1); //-Y
M(2*i,2) = ob_pt(2); //-Z
M(2*i,3) = 0;
M(2*i,4) = 0;
M(2*i,5) = 0;
M(2*i,6) = -ob_pt(0)*pr_pt(0); //Xx
M(2*i,7) = -ob_pt(1)*pr_pt(0); //Yx
M(2*i,8) = -ob_pt(2)*pr_pt(0); //Zx
M(2*i,9) = 1;
M(2*i,10) = 0;
M(2*i,11) = -pr_pt(0); //x
M(2*i+1,3) = ob_pt(0); //-X
M(2*i+1,4) = ob_pt(1); //-Y
M(2*i+1,5) = ob_pt(2); //-Z
M(2*i+1,0) = 0;
M(2*i+1,1) = 0;
M(2*i+1,2) = 0;
M(2*i+1,6) = -ob_pt(0)*pr_pt(1); //Xy
M(2*i+1,7) = -ob_pt(1)*pr_pt(1); //Yy
M(2*i+1,8) = -ob_pt(2)*pr_pt(1); //Zy
M(2*i+1,9) = 0;
M(2*i+1,10) = 1;
M(2*i+1,11) = -pr_pt(1); //y
}
//Compute M'M
Eigen::MatrixXf MTM;
MTM = M.transpose()*M;
//Do SVD of MTM
Eigen::JacobiSVD<Eigen::MatrixXf> svd(MTM, Eigen::ComputeThinU);
//Get the eigenvector corresponding to the smallest eigenvalue
Eigen::MatrixXf MatU = svd.matrixU();
int nz_num = svd.nonzeroSingularValues();
v = MatU.col(nz_num - 1);
float scale = v(0)*v(0)+v(1)*v(1)+v(2)*v(2)+v(3)*v(3)+v(4)*v(4)+v(5)*v(5)+v(6)*v(6)+v(7)*v(7)+v(8)*v(8);
scale/=3;
scale = pow(scale,0.5);
//Take care of scaling
v = v /scale;
//std::cout<<"Size of U"<<MatU.rows()<<" "<<MatU.cols()<<"\n";
std::cout<<"V: "<<v.transpose()<<"\n";
Eigen::Matrix3f rot_mat;
rot_mat<< v(0),v(1),v(2),v(3),v(4),v(5),v(6),v(7),v(8);
Eigen::Vector3f euler = rot_mat.eulerAngles(2,1,0);
//The vector to be returned carries 3 euler angles and 3 translation elements
Eigen::VectorXd x(6);
x<< v(9),v(10),v(11),euler(0),euler(1),euler(2);
return(x);
}
