Eigen::Vector3d CalibrateData::calibrate(Eigen::Vector3d data,
Eigen::Matrix3d alignment_matrix,
Eigen::Matrix3d sensitivity_matrix,
Eigen::Vector3d offset_vector)
{
Eigen::Vector3d calibrated_data;
calibrated_data = alignment_matrix.inverse() *
sensitivity_matrix.inverse() *
(data - offset_vector);
return calibrated_data;
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
const int numNodes = mesh->InitalVertices->rows();
// Compute deformation gradients
int numTets = mesh->Tetrahedra->rows();
Ms.resize(4,3*numTets);
MMTs.resize(4,4*numTets);
Eigen::Matrix<double,4,3> SelectorM;
SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
for(int t=0;t<numTets;t++)
{
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();
Eigen::Matrix3d V;
V << A-D,B-D,C-D;
Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
Ms.block<4,3>(0,3*t) = Mtemp;
MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
}
}
/*
* Given the line parameters [n_x,n_y,a_x,a_y] check if
* [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
*/
bool RSTEstimator::agree(std::vector<double> ¶meters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
Eigen::Matrix3d R;
Eigen::Vector3d T;
Eigen::Vector3d dif;
double E1,E2;
R << parameters[0] , parameters[1] , parameters[2],
parameters[3] , parameters[4] , parameters[5],
parameters[6] , parameters[7] , parameters[8];
T << parameters[9] , parameters[10] , parameters[11];
dif = data.first - R*data.second + T; //X21
E1 = dif.transpose()*dif;
dif = data.second - R.inverse() * (data.first-T); //X12
E2 = dif.transpose()*dif;
//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
return ( (E1 + E2) < this->deltaSquared);
}
Eigen::Matrix3d LinearAlgebra::invMatrixM(const Eigen::Matrix3d& M) {
Eigen::Matrix3d result;
// TODO: return the inverse of matrix M
result = M.inverse();
return result;
}
void KeyFrame::cam2Imu(Eigen::Vector3d T_c_w,
Eigen::Matrix3d R_c_w,
Eigen::Vector3d &t_w_i,
Eigen::Matrix3d &r_w_i)
{
r_w_i = (qic * R_c_w).inverse();
t_w_i = -R_c_w.inverse() * T_c_w - r_w_i * tic;
}
ON_NurbsSurface
FittingCylinder::initNurbsCylinderWithAxes (int order, NurbsDataSurface *data, Eigen::Matrix3d &axes)
{
Eigen::Vector3d mean;
unsigned s = unsigned (data->interior.size ());
mean = NurbsTools::computeMean (data->interior);
data->mean = mean;
Eigen::Vector3d v_max (0.0, 0.0, 0.0);
Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector3d p (axes.inverse () * (data->interior[i] - mean));
if (p (0) > v_max (0))
v_max (0) = p (0);
if (p (1) > v_max (1))
v_max (1) = p (1);
if (p (2) > v_max (2))
v_max (2) = p (2);
if (p (0) < v_min (0))
v_min (0) = p (0);
if (p (1) < v_min (1))
v_min (1) = p (1);
if (p (2) < v_min (2))
v_min (2) = p (2);
}
int ncpsU (order);
int ncpsV (2 * order + 4);
ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));
double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
double dcv = (2.0 * M_PI) / (ncpsV - order + 1);
double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));
Eigen::Vector3d cv_t, cv;
for (int i = 0; i < ncpsU; i++)
{
for (int j = 0; j < ncpsV; j++)
{
cv (0) = v_min (0) + dcu * i;
cv (1) = ry * sin (dcv * j);
cv (2) = rz * cos (dcv * j);
cv_t = axes * cv + mean;
nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
}
return nurbs;
}
const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic )
{
//ds get essential matrix
const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) );
//ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29
const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) );
return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse;
}
std::unique_ptr<Image> leastSquarePatch(const std::unique_ptr<Image>& image)
{
// process grayscale patches only
if (image->getChannelNum() > 1)
return std::unique_ptr<Image>();
auto data_ptr = image->getValues(0);
const auto w = image->getWidth();
const auto h = image->getHeight();
// least square fit to linear plane for light compensation
float xsum_orig = 0;
float ysum_orig = 0;
float csum_orig = 0;
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
xsum_orig += (i * data_ptr[j*w + i]);
ysum_orig += (j * data_ptr[j*w + i]);
csum_orig += (data_ptr[j*w + i]);
}
}
Eigen::Vector3d vsum(xsum_orig, ysum_orig, csum_orig);
float x2sum = 0, y2sum = 0, xysum = 0, xsum = 0, ysum = 0;
float csum = w*h;
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
x2sum += (i*i);
y2sum += (j*j);
xysum += (i*j);
xsum += i;
ysum += j;
}
}
Eigen::Matrix3d msum;
msum << x2sum, xysum, xsum,
xysum, y2sum, ysum,
xsum, ysum, csum;
auto vcoeff = msum.inverse() * vsum;
auto newImage = std::make_unique<Image>(w, h, 1);
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
(newImage->getValues(0))[j*w + i] = data_ptr[j*w + i]
- i*vcoeff[0] - j*vcoeff[1] - vcoeff[2];
}
}
return newImage;
}
int hormodular::Orientation::getRelativeOrientation(int connector, hormodular::Orientation localOrient, hormodular::Orientation remoteOrient)
{
//-- Create rotation matrices for local orientation:
Eigen::AngleAxisd rollAngle( deg2rad(localOrient.getRoll()), Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle( deg2rad(localOrient.getPitch()), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle( deg2rad(localOrient.getYaw()), Eigen::Vector3d::UnitX());
Eigen::Quaterniond q0 = yawAngle * pitchAngle * rollAngle ;
Eigen::Matrix3d rotationMatrix = q0.matrix();
//-- Create rotation matrices for the other orientation:
Eigen::AngleAxisd otherRollAngle( deg2rad(remoteOrient.getRoll()), Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd otherPitchAngle( deg2rad(remoteOrient.getPitch()), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd otherYawAngle( deg2rad(remoteOrient.getYaw()), Eigen::Vector3d::UnitX());
Eigen::Quaterniond q1 = otherYawAngle * otherPitchAngle * otherRollAngle;
Eigen::Matrix3d otherRotationMatrix = q1.matrix();
Eigen::Matrix3d relativeRotation = rotationMatrix.inverse() * otherRotationMatrix;
Eigen::Vector3d new_z = relativeRotation * Eigen::Vector3d::UnitZ();
// std::cout << "New_z: " << std::endl << new_z << std::endl << std::endl;
//-- Get connector base vector for the rotations:
Eigen::Vector3d base_vector;
if ( connector == 0 || connector == 2)
{
//-- Y axis
base_vector = Eigen::Vector3d::UnitY();
}
else if ( connector == 1 || connector == 3)
{
//-- X axis
base_vector = Eigen::Vector3d::UnitX();
}
//-- Try for rotations to fit the vector
for ( int i = 0; i < 4; i++)
{
Eigen::AngleAxisd rotAngle( deg2rad(i*90), base_vector);
Eigen::Matrix3d rotMatrix = rotAngle.matrix();
Eigen::Vector3d result_vector = rotMatrix * Eigen::Vector3d::UnitZ();
// std::cout << "i = " << i << std::endl << result_vector << std::endl << std::endl;
if ( vector_equal(result_vector, new_z) )
return i;
}
return -1;
}
// overloading constructor
ProcessLaserScan(ros::NodeHandle& nh,Eigen::Matrix3d initialPose, Eigen::Matrix3d neighborIP,std::string laserTopic)
{
nh_ = nh;
// initialize subscriber and publisher
scanSub_ = nh_.subscribe<sensor_msgs::LaserScan>(laserTopic, 1000, &ProcessLaserScan::laserScanCallback,this);
Eigen::Matrix3d relPose;
relPose = initialPose.inverse()*neighborIP;
minAngle = atan2(relPose(2-1,1-1),relPose(2-1,2-1));
minRange = sqrt(pow(relPose(1-1,3-1),2)+pow(relPose(2-1,3-1),2));
}
Vector3d get_robot_position(Vector3d position)
{
Rb2w = att_body.normalized().toRotationMatrix();
Rc2w = Rb2w * Rc2b;
Rc2i = Ri2w.inverse() * Rc2w;
//pos_i is normlized coordinate; the POS_I is the real coordinate; both are in intermidiate frame
Vector3d pos_i, POS_I, POS_W, CAM_W;
pos_i = Rc2i * position.normalized();
pos_i = pos_i / pos_i.z();
POS_I = pos_body.z() * pos_i;
CAM_W = pos_body + Rb2w * cam_off_b;
POS_W = Ri2w * POS_I + CAM_W;
return POS_W;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!link_model_)
return ConstraintEvaluationResult(true, 0.0);
const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
logWarn("No link in state with name '%s'", link_model_->getName().c_str());
return ConstraintEvaluationResult(false, 0.0);
}
Eigen::Vector3d xyz;
if (mobile_frame_)
{
Eigen::Matrix3d tmp;
tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2);
// 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
else
{
Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();
if (verbose)
{
Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
result ? "satisfied" : "violated", link_model_->getName().c_str(),
q_des.x(), q_des.y(), q_des.z(), q_des.w(),
q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
}
return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
bool isPositivelyDecomposable(const Point_3 &a, const Point_3 &b,
const Point_3 &c, const Point_3 &decomposed)
{
DEBUG_START;
Eigen::Matrix3d matrix;
matrix << a.x(), b.x(), c.x(),
a.y(), b.y(), c.y(),
a.z(), b.z(), c.z();
Eigen::Vector3d vector;
vector << decomposed.x(), decomposed.y(), decomposed.z();
Eigen::Vector3d coefficients = matrix.inverse() * vector;
std::cout << "Tried to decompose vector, result: " << std::endl
<< coefficients << std::endl;
DEBUG_END;
return coefficients(0) >= 0.
&& coefficients(1) >= 0.
&& coefficients(2) >= 0.;
}
bool extractPCFromColorModel(const PCRGB::Ptr& pc_in, PCRGB::Ptr& pc_out,
Eigen::Vector3d& mean, Eigen::Matrix3d& cov, double std_devs)
{
double hue_weight;
ros::param::param<double>("~hue_weight", hue_weight, 1.0);
printf("%f\n", hue_weight);
Eigen::Vector3d x, x_m;
Eigen::Matrix3d cov_inv = cov.inverse();
for(uint32_t i=0;i<pc_in->size();i++) {
extractHSL(pc_in->points[i].rgb, x(0), x(1), x(2));
x_m = x - mean;
x_m(0) *= hue_weight;
double dist = std::sqrt(x_m.transpose() * cov_inv * x_m);
if(dist <= std_devs)
pc_out->points.push_back(pc_in->points[i]);
}
}
void inverseTransform(Eigen::Vector3d &src, cspace config, Eigen::Vector3d &dest)
{
Eigen::Matrix3d rotationC;
rotationC << cos(config[5]), -sin(config[5]), 0,
sin(config[5]), cos(config[5]), 0,
0, 0, 1;
Eigen::Matrix3d rotationB;
rotationB << cos(config[4]), 0 , sin(config[4]),
0, 1, 0,
-sin(config[4]), 0, cos(config[4]);
Eigen::Matrix3d rotationA;
rotationA << 1, 0, 0 ,
0, cos(config[3]), -sin(config[3]),
0, sin(config[3]), cos(config[3]);
Eigen::Vector3d transitionV(config[0], config[1], config[2]);
Eigen::Matrix3d rotationM = rotationC * rotationB * rotationA;
dest = rotationM.inverse() * (src - transitionV);
}
void VoxelGrid<PointSourceType>::computeCentroidAndCovariance()
{
for (int i = 0; i < voxel_num_; i++) {
int ipoint_num = points_id_[i].size();
double point_num = static_cast<double>(ipoint_num);
Eigen::Vector3d pt_sum = centroid_[i];
if (ipoint_num > 0) {
centroid_[i] /= point_num;
}
if (ipoint_num >= min_points_per_voxel_) {
covariance_[i] = (covariance_[i] - 2 * (pt_sum * centroid_[i].transpose())) / point_num + centroid_[i] * centroid_[i].transpose();
covariance_[i] *= (point_num - 1.0) / point_num;
SymmetricEigensolver3x3 sv(covariance_[i]);
sv.compute();
Eigen::Matrix3d evecs = sv.eigenvectors();
Eigen::Matrix3d evals = sv.eigenvalues().asDiagonal();
if (evals(0, 0) < 0 || evals(1, 1) < 0 || evals(2, 2) <= 0) {
points_per_voxel_[i] = -1;
continue;
}
double min_cov_eigvalue = evals(2, 2) * 0.01;
if (evals(0, 0) < min_cov_eigvalue) {
evals(0, 0) = min_cov_eigvalue;
if (evals(1, 1) < min_cov_eigvalue) {
evals(1, 1) = min_cov_eigvalue;
}
covariance_[i] = evecs * evals * evecs.inverse();
}
icovariance_[i] = covariance_[i].inverse();
}
}
}
static Vector_3 leastSquaresPoint(const std::vector<unsigned> activeGroup,
const std::vector<SupportItem> &items)
{
DEBUG_START;
Eigen::Matrix3d matrix;
Eigen::Vector3d vector;
for (unsigned i = 0; i < 3; ++i)
{
vector(i) = 0.;
for (unsigned j = 0; j < 3; ++j)
matrix(i, j) = 0.;
}
std::cout << "Calculating least squares points for the following items"
<< std::endl;
for (unsigned iPlane : activeGroup)
{
SupportItem item = items[iPlane];
Vector_3 u = item.direction;
double value = item.value;
std::cout << " Item #" << iPlane << ": u = " << u << "; h = "
<< value << std::endl;
for (unsigned i = 0; i < 3; ++i)
{
for (unsigned j = 0; j < 3; ++j)
matrix(i, j) += u.cartesian(i)
* u.cartesian(j);
vector(i) += u.cartesian(i) * value;
}
}
Eigen::Vector3d solution = matrix.inverse() * vector;
Vector_3 result(solution(0), solution(1), solution(2));
std::cout << "Result: " << result << std::endl;
for (unsigned iPlane : activeGroup)
{
SupportItem item = items[iPlane];
double delta = item.direction * result - item.value;
std::cout << " delta = " << delta << std::endl;
}
DEBUG_END;
return result;
}
void Simulation :: dampVelocities( void )
{
// [ Mueller - 2007 Sec: 3.5 ]
for( unsigned mIdx = 0; mIdx < m_meshes.size(); ++mIdx ){
Vector x_cm;
Vector v_cm;
double sumMass = 0.;
for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
sumMass += v->mass;
x_cm += v->position * v->mass;
v_cm += v->velocity * v->mass;
}
x_cm /= sumMass;
v_cm /= sumMass;
Vector r;
Vector L;
Eigen::Matrix3d I = Eigen::Matrix3d::Zero();
for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
r = v->position - x_cm;
L += cross( r, v->mass * v->velocity );
Eigen::Matrix3d singleI = Eigen::Matrix3d::Zero();
singleI << 0., r[2], -r[1],
-r[2], 0., r[0],
r[2], -r[0], 0.;
I += singleI * singleI.transpose() * v->mass;
}
Eigen::Vector3d L_tmp ( L[0], L[1], L[2] );
Eigen::Vector3d omegaTemp = I.inverse() * L_tmp;
Vector ang_vel_omega( omegaTemp[0], omegaTemp[1], omegaTemp[2] );
for( VertexIter v = m_meshes[mIdx]->vertices.begin(); v != m_meshes[mIdx]->vertices.end(); ++v ){
r = v->position - x_cm;
Vector delta_v = v_cm + cross( ang_vel_omega, r ) - v->velocity;
v->velocity = v->velocity + ( m_meshes[mIdx]->dampingStiffness() * delta_v );
}
}
}
void KeyFrame::initPtsByReprojection(Eigen::Vector3d Ti_predict,
Eigen::Matrix3d Ri_predict,
std::vector<cv::Point2f> &measurements_predict)
{
measurements_predict.clear();
Vector3d pts_predict;
for(int i = 0; i < (int)point_clouds.size(); i++)
{
Eigen::Vector3d pts_w = point_clouds[i];
Eigen::Vector3d pts_imu_j = Ri_predict.inverse() * (pts_w - Ti_predict);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
pts_predict << pts_camera_j.x()/pts_camera_j.z(),
pts_camera_j.y()/pts_camera_j.z(),
1.0;
Vector2d point_uv;
point_uv.x() = FOCUS_LENGTH_X * pts_predict.x() + PX;
point_uv.y() = FOCUS_LENGTH_Y * pts_predict.y() + PY;
measurements_predict.push_back(cv::Point2f(point_uv.x(), point_uv.y()));
}
if(measurements_predict.size() == 0)
{
measurements_predict = measurements;
}
}
template <typename PointSource, typename PointTarget> inline void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
using namespace std;
// Difference between consecutive transforms
double delta = 0;
// Get the size of the target
const size_t N = indices_->size ();
// Set the mahalanobis matrices to identity
mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
// Compute target cloud covariance matrices
if (target_covariances_.empty ())
computeCovariances<PointTarget> (target_, tree_, target_covariances_);
// Compute input cloud covariance matrices
if (input_covariances_.empty ())
computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_);
base_transformation_ = guess;
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
while(!converged_)
{
size_t cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// guess corresponds to base_t and transformation_ to t
Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
for(size_t i = 0; i < 4; i++)
for(size_t j = 0; j < 4; j++)
for(size_t k = 0; k < 4; k++)
transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
for (size_t i = 0; i < N; i++)
{
PointSource query = output[i];
query.getVector4fMap () = guess * query.getVector4fMap ();
query.getVector4fMap () = transformation_ * query.getVector4fMap ();
if (!searchForNeighbors (query, nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
Eigen::Matrix3d &C1 = input_covariances_[i];
Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
Eigen::Matrix3d &M = mahalanobis_[i];
// M = R*C1
M = R * C1;
// temp = M*R' + C2 = R*C1*R' + C2
Eigen::Matrix3d temp = M * R.transpose();
temp+= C2;
// M = temp^-1
M = temp.inverse ();
source_indices[cnt] = static_cast<int> (i);
target_indices[cnt] = nn_indices[0];
cnt++;
}
}
// Resize to the actual number of valid correspondences
source_indices.resize(cnt); target_indices.resize(cnt);
/* optimize transformation using the current assignment and Mahalanobis metrics*/
previous_transformation_ = transformation_;
//optimization right here
try
{
rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
/* compute the delta from this iteration */
delta = 0.;
for(int k = 0; k < 4; k++) {
for(int l = 0; l < 4; l++) {
double ratio = 1;
if(k < 3 && l < 3) // rotation part of the transform
ratio = 1./rotation_epsilon_;
else
ratio = 1./transformation_epsilon_;
double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
if(c_delta > delta)
delta = c_delta;
}
}
}
catch (PCLException &e)
{
PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
break;
}
nr_iterations_++;
// Check for convergence
if (nr_iterations_ >= max_iterations_ || delta < 1)
{
converged_ = true;
previous_transformation_ = transformation_;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
}
else
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
}
//for some reason the static equivalent methode raises an error
// final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
// final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
}
ON_NurbsSurface
FittingCylinder::initNurbsPCACylinder (int order, NurbsDataSurface *data)
{
Eigen::Vector3d mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = unsigned (data->interior.size ());
NurbsTools::pca (data->interior, mean, eigenvectors, eigenvalues);
data->mean = mean;
data->eigenvectors = eigenvectors;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
Eigen::Vector3d v_max (0.0, 0.0, 0.0);
Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector3d p (eigenvectors.inverse () * (data->interior[i] - mean));
if (p (0) > v_max (0))
v_max (0) = p (0);
if (p (1) > v_max (1))
v_max (1) = p (1);
if (p (2) > v_max (2))
v_max (2) = p (2);
if (p (0) < v_min (0))
v_min (0) = p (0);
if (p (1) < v_min (1))
v_min (1) = p (1);
if (p (2) < v_min (2))
v_min (2) = p (2);
}
int ncpsU (order);
int ncpsV (2 * order + 4);
ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));
double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
double dcv = (2.0 * M_PI) / (ncpsV - order + 1);
double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));
Eigen::Vector3d cv_t, cv;
for (int i = 0; i < ncpsU; i++)
{
for (int j = 0; j < ncpsV; j++)
{
cv (0) = v_min (0) + dcu * i;
cv (1) = ry * sin (dcv * j);
cv (2) = rz * cos (dcv * j);
cv_t = eigenvectors * cv + mean;
nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
}
return nurbs;
}
ON_NurbsSurface
FittingSurface::initNurbsPCABoundingBox (int order, NurbsDataSurface *m_data, Eigen::Vector3d z)
{
Eigen::Vector3d mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = m_data->interior.size ();
m_data->interior_param.clear ();
NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
m_data->mean = mean;
m_data->eigenvectors = eigenvectors;
bool flip (false);
if (eigenvectors.col (2).dot (z) < 0.0)
flip = true;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
Eigen::Matrix3d eigenvectors_inv = eigenvectors.inverse ();
Eigen::Vector3d v_max (0.0, 0.0, 0.0);
Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector3d p = eigenvectors_inv * (m_data->interior[i] - mean);
m_data->interior_param.push_back (Eigen::Vector2d (p (0), p (1)));
if (p (0) > v_max (0))
v_max (0) = p (0);
if (p (1) > v_max (1))
v_max (1) = p (1);
if (p (2) > v_max (2))
v_max (2) = p (2);
if (p (0) < v_min (0))
v_min (0) = p (0);
if (p (1) < v_min (1))
v_min (1) = p (1);
if (p (2) < v_min (2))
v_min (2) = p (2);
}
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector2d &p = m_data->interior_param[i];
if (v_max (0) > v_min (0) && v_max (0) > v_min (0))
{
p (0) = (p (0) - v_min (0)) / (v_max (0) - v_min (0));
p (1) = (p (1) - v_min (1)) / (v_max (1) - v_min (1));
}
else
{
throw std::runtime_error ("[NurbsTools::initNurbsPCABoundingBox] Error: v_max <= v_min");
}
}
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
double dcu = (v_max (0) - v_min (0)) / (nurbs.Order (0) - 1);
double dcv = (v_max (1) - v_min (1)) / (nurbs.Order (1) - 1);
Eigen::Vector3d cv_t, cv;
for (int i = 0; i < nurbs.Order (0); i++)
{
for (int j = 0; j < nurbs.Order (1); j++)
{
cv (0) = v_min (0) + dcu * i;
cv (1) = v_min (1) + dcv * j;
cv (2) = 0.0;
cv_t = eigenvectors * cv + mean;
if (flip)
nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
else
nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
}
}
return nurbs;
}
ON_NurbsSurface
FittingSurface::initNurbsPCABoundingBox (int order, NurbsDataSurface *m_data, ON_3dVector z)
{
ON_3dVector mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = m_data->interior.size ();
m_data->interior_param.clear ();
NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
m_data->mean = mean;
//m_data->eigenvectors = (*eigenvectors);
bool flip (false);
Eigen::Vector3d ez(z[0],z[1],z[2]);
if (eigenvectors.col (2).dot (ez) < 0.0)
flip = true;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
Eigen::Matrix3d eigenvectors_inv = eigenvectors.inverse ();
ON_3dVector v_max(0.0, 0.0, 0.0);
ON_3dVector v_min(DBL_MAX, DBL_MAX, DBL_MAX);
Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
for (unsigned i = 0; i < s; i++)
{
Eigen::Vector3d eint(m_data->interior[i][0], m_data->interior[i][1], m_data->interior[i][2]);
Eigen::Vector3d ep = eigenvectors_inv * (eint - emean);
ON_3dPoint p(ep (0), ep (1), ep(2));
m_data->interior_param.push_back (ON_2dPoint(p[0], p[1]));
if (p[0] > v_max[0])
v_max[0] = p[0];
if (p[1] > v_max[1])
v_max[1] = p[1];
if (p[2] > v_max[2])
v_max[2] = p[2];
if (p[0] < v_min[0])
v_min[0] = p[0];
if (p[1] < v_min[1])
v_min[1] = p[1];
if (p[2] < v_min[2])
v_min[2] = p[2];
}
for (unsigned i = 0; i < s; i++)
{
ON_2dVector &p = m_data->interior_param[i];
if (v_max[0] > v_min[0] && v_max[0] > v_min[0])
{
p[0] = (p[0] - v_min[0]) / (v_max[0] - v_min[0]);
p[1] = (p[1] - v_min[1]) / (v_max[1] - v_min[1]);
}
else
{
throw std::runtime_error ("[NurbsTools::initNurbsPCABoundingBox] Error: v_max <= v_min");
}
}
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
double dcu = (v_max[0] - v_min[0]) / (nurbs.Order (0) - 1);
double dcv = (v_max[1] - v_min[1]) / (nurbs.Order (1) - 1);
ON_3dPoint cv_t, cv;
Eigen::Vector3d ecv_t2, ecv2;
Eigen::Vector3d emean2(mean[0],mean[1],mean[2]);
for (int i = 0; i < nurbs.Order (0); i++)
{
for (int j = 0; j < nurbs.Order (1); j++)
{
cv[0] = v_min[0] + dcu * i;
cv[1] = v_min[1] + dcv * j;
cv[2] = 0.0;
ecv2 (0) = cv[0];
ecv2 (1) = cv[1];
ecv2 (2) = cv[2];
ecv_t2 = eigenvectors * ecv2 + emean2;
if (flip)
nurbs.SetCV (nurbs.Order (0) - 1 - i, j, cv_t);
else
nurbs.SetCV (i, j, cv_t);
}
}
return nurbs;
}
void Node::detect3DLines(const cv::Mat& gray_uchar, const cv::Mat& depth_float, double line2d_len_thres,
const cv::Mat& K,double ratio_of_collinear_pts, double line_3d_len_thres_m, double depth_scaling, string algorithm)
{
// MyTimer tm; tm.start();
vector<FrameLine> allLines; // 2d and 3d lines
if(algorithm == "LSD") { // using LSD, 100+ ms
IplImage pImg = gray_uchar;
ntuple_list lsdOut;
lsdOut = callLsd(&pImg);// use LSD method
int dim = lsdOut->dim;
double a,b,c,d;
allLines.reserve(lsdOut->size);
for(int i=0; i<lsdOut->size; i++) {// store LSD output to lineSegments
a = lsdOut->values[i*dim];
b = lsdOut->values[i*dim+1];
c = lsdOut->values[i*dim+2];
d = lsdOut->values[i*dim+3];
if ( sqrt((a-c)*(a-c)+(b-d)*(b-d)) > line2d_len_thres) {
allLines.push_back(FrameLine(cv::Point2d(a,b), cv::Point2d(c,d)));
}
}
}
if(algorithm == "EDLINES") { // using EDlines, 15 ms
int n;
LS* ls = callEDLines(gray_uchar, &n);
allLines.reserve(n);
for(int i=0; i<n; i++) {// store output to lineSegments
if ((ls[i].sx-ls[i].ex)*(ls[i].sx-ls[i].ex) +(ls[i].sy-ls[i].ey)*(ls[i].sy-ls[i].ey)
> line2d_len_thres*line2d_len_thres) {
allLines.push_back(FrameLine(cv::Point2d(ls[i].sx,ls[i].sy), cv::Point2d(ls[i].ex,ls[i].ey)));
}
}
}
Eigen::Matrix3d eK;
for(int i=0; i<3; ++i)
for(int j=0; j<3; ++j)
eK(i,j) = K.at<double>(i,j);
Eigen::Matrix3d Kinv = eK.inverse();
int depth_CVMatDepth = depth_float.depth();
#pragma omp parallel for
for(int i=0; i<allLines.size(); ++i) { // 20 -30 ms
double len = cv::norm(allLines[i].p - allLines[i].q);
// iterate through a line
double numSmp = min(max(len/sysPara.line_sample_interval, (double)sysPara.line_sample_min_num), (double)sysPara.line_sample_max_num); // smaller numSmp for fast speed, larger numSmp should be more accurate
vector<cv::Point3d> pts3d; pts3d.reserve(numSmp);
for(int j=0; j<=numSmp; ++j) {
// use nearest neighbor to querry depth value
// assuming position (0,0) is the top-left corner of image, then the
// top-left pixel's center would be (0.5,0.5)
cv::Point2d pt = allLines[i].p * (1-j/numSmp) + allLines[i].q * (j/numSmp);
if(pt.x<0 || pt.y<0 || pt.x >= depth_float.cols || pt.y >= depth_float.rows ) continue;
int row, col; // nearest pixel for pt
if((floor(pt.x) == pt.x) && (floor(pt.y) == pt.y)) {// boundary issue
col = max(int(pt.x-1),0);
row = max(int(pt.y-1),0);
} else {
col = int(pt.x);
row = int(pt.y);
}
double zval = -1;
double depval;
if(depth_CVMatDepth == CV_32F)
depval = depth_float.at<float>(row,col);
else if (depth_CVMatDepth == CV_64F)
depval = depth_float.at<double>(row,col);
else {
cerr<<"Node::extractLineDepth: depth image matrix type is not float/double\n";
exit(0);
}
if(depval < EPS || isnan((float)depval)) { // no depth info
} else {
zval = depval/depth_scaling; // in meter, z-value
}
if (zval > 0 ) {
Eigen::Vector3d ept(pt.x, pt.y, 1);
Eigen::Vector3d xy3d = Kinv * ept;
xy3d = xy3d/xy3d(2);
pts3d.push_back(cv::Point3d(xy3d(0)*zval, xy3d(1)*zval, zval));
}
}
if (pts3d.size() < max(10.0, numSmp *ratio_of_collinear_pts))
continue;
RandomLine3d tmpLine;
vector<RandomPoint3d> rndpts3d;
rndpts3d.reserve(pts3d.size());
// compute uncertainty of 3d points
for(int j=0; j<pts3d.size();++j) {
rndpts3d.push_back(compPt3dCov(pts3d[j], K, asynch_time_diff_sec_));
}
// using ransac to extract a 3d line from 3d pts
tmpLine = extract3dline_mahdist(rndpts3d);
if(tmpLine.pts.size()/numSmp > ratio_of_collinear_pts &&
cv::norm(tmpLine.A - tmpLine.B) > line_3d_len_thres_m) {
allLines[i].haveDepth = true;
allLines[i].line3d = tmpLine;
}
}
//// prepare for compute msld line descriptors
cv::Mat xGradImg, yGradImg;
int ddepth = CV_64F;
cv::Sobel(gray_uchar, xGradImg, ddepth, 1, 0, 5); // Gradient X
cv::Sobel(gray_uchar, yGradImg, ddepth, 0, 1, 5); // Gradient Y
double *xG, *yG;
if(xGradImg.isContinuous() && yGradImg.isContinuous()) {
xG = (double*) xGradImg.data;
yG = (double*) yGradImg.data;
} else {
xG = new double[xGradImg.rows*xGradImg.cols];
yG = new double[yGradImg.rows*yGradImg.cols];
int idx = 0;
for(int i=0; i<xGradImg.rows; ++i) {
for(int j=0; j<xGradImg.cols; ++j) {
xG[idx] = xGradImg.at<double>(i,j);
yG[idx] = yGradImg.at<double>(i,j);
++idx;
}
}
}
lines.reserve(allLines.size());
// NOTE this for-loop mustnot be parallized
for(int i=0; i<allLines.size(); ++i) {
if(allLines[i].haveDepth) {
lines.push_back(allLines[i]);
lines.back().lid = lines.size()-1;
lines.back().complineEq2d();
lines.back().r = lines.back().getGradient(&xGradImg, &yGradImg);
}
}
#pragma omp parallel for
for(int i=0; i<lines.size(); ++i) { // 60 ms, 0.6ms/line
computeMSLD(lines[i], xG, yG, gray_uchar.cols, gray_uchar.rows); // 0.1 ms/line
MLEstimateLine3d(lines[i].line3d, sysPara.line3d_mle_iter_num);
vector<RandomPoint3d> ().swap(lines[i].line3d.pts); // swap with a empty vector effectively free up the memory, clear() doesn't.
}
// tm.end(); cout<<"detect 3d lines "<<tm.time_ms<<" ms\n";
if(!xGradImg.isContinuous() || !yGradImg.isContinuous()) { // dynamic allocation
delete[] xG;
delete[] yG;
}
}
bool MyWindow::transformSegmentAtSample(std::string segmentName,
int timeStep)
{
Eigen::VectorXd segmentConfig;
if (mInputMotion->getSegmentConfig(timeStep, segmentName, &segmentConfig))
{
if (segmentName == "root")
{
// Transform Root
Eigen::Vector6d r_t = segmentConfig;
// r_t: head(3)->translation, tail(3)->rotation
// FreeJoint: head(3)->rotation, tail(3)->translation
Eigen::Isometry3d tf;
tf.linear() = dart::math::eulerXYZToMatrix(r_t.tail(3)*deg_to_rad);
//tf.linear() = dart::math::eulerZYXToMatrix(r_t.tail(3)*deg_to_rad);
tf.translation() = r_t.head(3)*unit;
mSkel->getJoint("root_joint")
->setPositions(dart::dynamics::FreeJoint::convertToPositions(tf));
return true;
}
else
{
// other bones
// prepare reference frame
Eigen::Vector3d segmentAxes;
if (mAsfData->getSegmentAxes(segmentName, &segmentAxes))
{
Eigen::Matrix3d refFrame = dart::math::eulerXYZToMatrix(segmentAxes*deg_to_rad);
//Eigen::Matrix3d refFrame = dart::math::eulerZYXToMatrix(segmentAxes*deg_to_rad);
// calculate motion transformation
Eigen::Matrix3d motionRot = dart::math::eulerXYZToMatrix(segmentConfig*deg_to_rad);
//Eigen::Matrix3d motionRot = dart::math::eulerZYXToMatrix(segmentConfig*deg_to_rad);
Eigen::Isometry3d finalTF;
//finalTF.linear() = refFrame.inverse() * motionRot * refFrame;
finalTF.linear() = refFrame * motionRot * refFrame.inverse();
//finalTF.linear() = motionRot;
// transform
mSkel->getBodyNode(segmentName)->getParentBodyNode()->getParentJoint()
->setPositions(dart::dynamics::BallJoint::convertToPositions(finalTF.linear()));
std::cout << segmentName << std::endl;
std::cout << mSkel->getBodyNode(segmentName)->getParentJoint()
->getName() << std::endl;
std::cout << mSkel->getBodyNode(segmentName)->getRelativeTransform().linear()
<< std::endl;
std::cout << mSkel->getJoint(segmentName+"_joint")->getPositions() << std::endl;
return true;
}
std::cout << "segment transform fail" << std::endl;
return false;
}
}
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;
}
int Hsh::loadData(FILE* file, int width, int height, int basisTerm, bool urti, CallBackPos * cb,const QString& text)
{
type = "HSH";
w = width;
h = height;
ordlen = basisTerm;
bands = 3;
fread(gmin, sizeof(float), basisTerm, file);
fread(gmax, sizeof(float), basisTerm, file);
if (feof(file))
return -1;
int offset = 0;
int size = w * h * basisTerm;
float* redPtr = new float[size];
float* greenPtr = new float[size];
float* bluePtr = new float[size];
unsigned char c;
if (!urti)
{
for(int j = 0; j < h; j++)
{
if (cb != NULL && j % 50 == 0)(*cb)(j * 50.0 / h, text);
for(int i = 0; i < w; i++)
{
offset = j * w + i;
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
}
}
}
else
{
for(int j = 0; j < h; j++)
{
if (cb != NULL && j % 50 == 0)(*cb)(j * 50 / h, text);
for(int i = 0; i < w; i++)
{
offset = j * w + i;
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
}
}
}
fclose(file);
mipMapSize[0] = QSize(w, h);
redCoefficients.setLevel(redPtr, size, 0);
greenCoefficients.setLevel(greenPtr, size, 0);
blueCoefficients.setLevel(bluePtr, size, 0);
// Computes mip-mapping.
if (cb != NULL) (*cb)(50, "Mip mapping generation...");
for (int level = 1; level < MIP_MAPPING_LEVELS; level++)
{
int width = mipMapSize[level - 1].width();
int height = mipMapSize[level - 1].height();
int width2 = ceil(width / 2.0);
int height2 = ceil(height / 2.0);
size = width2*height2*basisTerm;
redCoefficients.setLevel(new float[size], size, level);
greenCoefficients.setLevel(new float[size], size, level);
blueCoefficients.setLevel(new float[size], size, level);
int th_id;
#pragma omp parallel for
for (int i = 0; i < height - 1; i+=2)
{
th_id = omp_get_thread_num();
if (th_id == 0)
{
if (cb != NULL && i % 50 == 0) (*cb)(50 + (level-1)*8 + i*8.0/height, "Mip mapping generation...");
}
for (int j = 0; j < width - 1; j+=2)
{
int index1 = (i * width + j)*ordlen;
int index2 = (i * width + j + 1)*ordlen;
int index3 = ((i + 1) * width + j)*ordlen;
int index4 = ((i + 1) * width + j + 1)*ordlen;
int offset = (i/2 * width2 + j/2)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
}
}
}
if (width2 % 2 != 0)
{
for (int i = 0; i < height - 1; i+=2)
{
int index1 = ((i + 1) * width - 1)*ordlen;
int index2 = ((i + 2) * width - 1)*ordlen;
int offset = ((i/2 + 1) * width2 - 1)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
}
}
}
if (height % 2 != 0)
{
for (int i = 0; i < width - 1; i+=2)
{
int index1 = ((height - 1) * width + i)*ordlen;
int index2 = ((height - 1) * width + i + 1)*ordlen;
int offset = ((height2 - 1) * width2 + i/2)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
}
}
}
if (height % 2 != 0 && width % 2 != 0)
{
int index1 = (height*width - 1)*ordlen;
int offset = (height2*width2 - 1)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k);
}
}
mipMapSize[level] = QSize(width2, height2);
}
//Compute normals.
if (cb != NULL) (*cb)(75 , "Normals generation...");
Eigen::Vector3d l0(sin(M_PI/4)*cos(M_PI/6), sin(M_PI/4)*sin(M_PI/6), cos(M_PI/4));
Eigen::Vector3d l1(sin(M_PI/4)*cos(5*M_PI / 6), sin(M_PI/4)*sin(5*M_PI / 6), cos(M_PI/4));
Eigen::Vector3d l2(sin(M_PI/4)*cos(3*M_PI / 2), sin(M_PI/4)*sin(3*M_PI / 2), cos(M_PI/4));
float hweights0[16], hweights1[16], hweights2[16];
getHSH(M_PI / 4, M_PI / 6, hweights0, ordlen);
getHSH(M_PI / 4, 5*M_PI / 6, hweights1, ordlen);
getHSH(M_PI / 4, 3*M_PI / 2, hweights2, ordlen);
Eigen::Matrix3d L;
L.setIdentity();
L.row(0) = l0;
L.row(1) = l1;
L.row(2) = l2;
Eigen::Matrix3d LInverse = L.inverse();
for (int level = 0; level < MIP_MAPPING_LEVELS; level++)
{
const float* rPtr = redCoefficients.getLevel(level);
const float* gPtr = greenCoefficients.getLevel(level);
const float* bPtr = blueCoefficients.getLevel(level);
vcg::Point3f* temp = new vcg::Point3f[mipMapSize[level].width()*mipMapSize[level].height()];
if (cb != NULL) (*cb)(75 + level*6, "Normal generation...");
#pragma omp parallel for
for (int y = 0; y < mipMapSize[level].height(); y++)
{
for (int x = 0; x < mipMapSize[level].width(); x++)
{
int offset= y * mipMapSize[level].width() + x;
Eigen::Vector3d f(0, 0, 0);
for (int k = 0; k < ordlen; k++)
{
f(0) += rPtr[offset*ordlen + k] * hweights0[k];
f(1) += rPtr[offset*ordlen + k] * hweights1[k];
f(2) += rPtr[offset*ordlen + k] * hweights2[k];
}
for (int k = 0; k < ordlen; k++)
{
f(0) += gPtr[offset*ordlen + k] * hweights0[k];
f(1) += gPtr[offset*ordlen + k] * hweights1[k];
f(2) += gPtr[offset*ordlen + k] * hweights2[k];
}
for (int k = 0; k < ordlen; k++)
{
f(0) += bPtr[offset*ordlen + k] * hweights0[k];
f(1) += bPtr[offset*ordlen + k] * hweights1[k];
f(2) += bPtr[offset*ordlen + k] * hweights2[k];
}
f /= 3.0;
Eigen::Vector3d normal = LInverse * f;
temp[offset] = vcg::Point3f(normal(0), normal(1), normal(2));
temp[offset].Normalize();
}
}
normals.setLevel(temp, mipMapSize[level].width()*mipMapSize[level].height(), level);
}
return 0;
}
int main(int /*argc*/, char** /*argv*/)
{
try
{
imp::ImageCv32fC1::Ptr cv_im0;
imp::cvBridgeLoad(cv_im0,
"/home/mwerlberger/data/epipolar_stereo_test/00000.png",
imp::PixelOrder::gray);
imp::ImageCv32fC1::Ptr cv_im1;
imp::cvBridgeLoad(cv_im1,
"/home/mwerlberger/data/epipolar_stereo_test/00001.png",
imp::PixelOrder::gray);
// rectify images for testing
imp::cu::ImageGpu32fC1::Ptr im0 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im0);
imp::cu::ImageGpu32fC1::Ptr im1 = std::make_shared<imp::cu::ImageGpu32fC1>(*cv_im1);
Eigen::Quaterniond q_world_im0(0.14062777, 0.98558398, 0.02351040, -0.09107859);
Eigen::Quaterniond q_world_im1(0.14118687, 0.98569744, 0.01930722, -0.08996696);
Eigen::Vector3d t_world_im0(-0.12617580, 0.50447008, 0.15342121);
Eigen::Vector3d t_world_im1(-0.11031053, 0.50314023, 0.15158643);
imp::cu::PinholeCamera cu_cam(414.09, 413.799, 355.567, 246.337);
// im0: fixed image; im1: moving image
imp::cu::Matrix3f F_fix_mov;
imp::cu::Matrix3f F_mov_fix;
Eigen::Matrix3d F_fm, F_mf;
{ // compute fundamental matrix
Eigen::Matrix3d R_world_mov = q_world_im1.matrix();
Eigen::Matrix3d R_world_fix = q_world_im0.matrix();
Eigen::Matrix3d R_fix_mov = R_world_fix.inverse()*R_world_mov;
// in ref coordinates
Eigen::Vector3d t_fix_mov = R_world_fix.inverse()*(-t_world_im0 + t_world_im1);
Eigen::Matrix3d tx_fix_mov;
tx_fix_mov << 0, -t_fix_mov[2], t_fix_mov[1],
t_fix_mov[2], 0, -t_fix_mov[0],
-t_fix_mov[1], t_fix_mov[0], 0;
Eigen::Matrix3d E_fix_mov = tx_fix_mov * R_fix_mov;
Eigen::Matrix3d K;
K << cu_cam.fx(), 0, cu_cam.cx(),
0, cu_cam.fy(), cu_cam.cy(),
0, 0, 1;
Eigen::Matrix3d Kinv = K.inverse();
F_fm = Kinv.transpose() * E_fix_mov * Kinv;
F_mf = F_fm.transpose();
} // end .. compute fundamental matrix
// convert the Eigen-thingy to something that we can use in CUDA
for(size_t row=0; row<F_fix_mov.rows(); ++row)
{
for(size_t col=0; col<F_fix_mov.cols(); ++col)
{
F_fix_mov(row,col) = (float)F_fm(row,col);
F_mov_fix(row,col) = (float)F_mf(row,col);
}
}
// compute SE3 transformation
imp::cu::SE3<float> T_world_fix(
static_cast<float>(q_world_im0.w()), static_cast<float>(q_world_im0.x()),
static_cast<float>(q_world_im0.y()), static_cast<float>(q_world_im0.z()),
static_cast<float>(t_world_im0.x()), static_cast<float>(t_world_im0.y()),
static_cast<float>(t_world_im0.z()));
imp::cu::SE3<float> T_world_mov(
static_cast<float>(q_world_im1.w()), static_cast<float>(q_world_im1.x()),
static_cast<float>(q_world_im1.y()), static_cast<float>(q_world_im1.z()),
static_cast<float>(t_world_im1.x()), static_cast<float>(t_world_im1.y()),
static_cast<float>(t_world_im1.z()));
imp::cu::SE3<float> T_mov_fix = T_world_mov.inv() * T_world_fix;
// end .. compute SE3 transformation
std::cout << "T_mov_fix:\n" << T_mov_fix << std::endl;
std::unique_ptr<imp::cu::VariationalEpipolarStereo> stereo(
new imp::cu::VariationalEpipolarStereo());
stereo->parameters()->verbose = 1;
stereo->parameters()->solver = imp::cu::StereoPDSolver::EpipolarPrecondHuberL1;
stereo->parameters()->ctf.scale_factor = 0.8f;
stereo->parameters()->ctf.iters = 30;
stereo->parameters()->ctf.warps = 5;
stereo->parameters()->ctf.apply_median_filter = true;
stereo->parameters()->lambda = 20;
stereo->addImage(im0);
stereo->addImage(im1);
imp::cu::ImageGpu32fC1::Ptr cu_mu = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size());
imp::cu::ImageGpu32fC1::Ptr cu_sigma2 = std::make_shared<imp::cu::ImageGpu32fC1>(im0->size());
cu_mu->setValue(-5.f);
cu_sigma2->setValue(0.0f);
stereo->setFundamentalMatrix(F_mov_fix);
stereo->setIntrinsics({cu_cam, cu_cam});
stereo->setExtrinsics(T_mov_fix);
stereo->setDepthProposal(cu_mu, cu_sigma2);
stereo->solve();
std::shared_ptr<imp::cu::ImageGpu32fC1> d_disp = stereo->getDisparities();
{
imp::Pixel32fC1 min_val,max_val;
imp::cu::minMax(*d_disp, min_val, max_val);
std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl;
}
imp::cu::cvBridgeShow("im0", *im0);
imp::cu::cvBridgeShow("im1", *im1);
// *d_disp *= -1;
{
imp::Pixel32fC1 min_val,max_val;
imp::cu::minMax(*d_disp, min_val, max_val);
std::cout << "disp: min: " << min_val.x << " max: " << max_val.x << std::endl;
}
imp::cu::cvBridgeShow("disparities", *d_disp, -18.0f, 11.0f);
imp::cu::cvBridgeShow("disparities minmax", *d_disp, true);
cv::waitKey();
}
catch (std::exception& e)
{
std::cout << "[exception] " << e.what() << std::endl;
assert(false);
}
return EXIT_SUCCESS;
}
