Matrix3f Segment::jacobianMatrix() {
Matrix3f result;
result.col(0) = jacobianColumn(getX());
result.col(1) = jacobianColumn(getY());
result.col(2) = jacobianColumn(getZ());
return result;
}
/**************************************************************************************************
* Procecure *
* *
* Description: swapColumns *
* Class : ApproachMovementSpace *
**************************************************************************************************/
Matrix3f ApproachMovementSpace::swapColumns(Matrix3f matrix, uint column1, uint column2)
{
Vector3f aux = matrix.col(column1);
matrix.col(column1) = matrix.col(column2);
matrix.col(column2) = aux;
return matrix;
}
SceneObject& SceneObject::setRotation(const Vector3f& x, const Vector3f& y, const Vector3f& z) {
Matrix3f M;
M.col(0) = x;
M.col(1) = y;
M.col(2) = z;
rotation = Quaternionf(M);
rotation.normalize();
return *this;
}
void FakeKinect::setWorldFromCam(const Eigen::Affine3f &worldFromCam) {
Vector3f eye, center, up;
Matrix3f rot;
rot = worldFromCam.linear();
eye = worldFromCam.translation();
center = rot.col(2);
up = -rot.col(1);
m_cam->setViewMatrixAsLookAt(toOSGVector(eye), toOSGVector(center+eye), toOSGVector(up));
}
/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3vMFCF::evalCostFunction(Matrix3f& R)
{
float c = 0.0f;
for (uint32_t j=0; j<6; ++j) {
if(j%2 ==0){
c -= this->cld_.xSums().col(j).transpose()*R.col(j/2);
}else{
c += this->cld_.xSums().col(j).transpose()*R.col(j/2);
}
}
return c;
}
void Camera::setDirection(const Vector3f& newDirection)
{
// TODO implement it computing the rotation between newDirection and current dir ?
Vector3f up = this->up();
Matrix3f camAxes;
camAxes.col(2) = (-newDirection).normalized();
camAxes.col(0) = up.cross( camAxes.col(2) ).normalized();
camAxes.col(1) = camAxes.col(2).cross( camAxes.col(0) ).normalized();
setOrientation(Quaternionf(camAxes));
mViewIsUptodate = false;
}
void Camera::lookAt(const Vector3f ¢er, const Vector3f &eye, Vector3f &up) {
Vector3f z = eye - center;
z.normalize();
up.normalize();
Vector3f x = up.cross(z);
x.normalize();
Vector3f y = z.cross(x);
Matrix3f M;
M.col(0) = x;
M.col(1) = y;
M.col(2) = z;
std::cout << M << std::endl;
rotation = Quaternionf(M);
position = eye;
}
int main(int, char**)
{
cout.precision(3);
Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;
std::cout << m;
return 0;
}
void RealtimeMF_openni::normals_cb(float* d_normals, uint8_t* haveData,
uint32_t w, uint32_t h)
{
tLog_.tic(-1); // reset all timers
int32_t nComp = 0;
float* d_nComp = this->normalExtract->d_normalsComp(nComp);
Matrix3f kRwBefore = kRw_;
tLog_.toctic(0,1);
optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0);
double residual = optSO3_->conjugateGradientCUDA(kRw_,nCGIter_);
double D_KL = optSO3_->D_KL_axisUnif();
tLog_.toctic(1,2);
{
boost::mutex::scoped_lock updateLock(this->updateModelMutex);
this->normalsImg_ = this->normalExtract->normalsImg();
if(z_.rows() != w*h) z_.resize(w*h);
this->normalExtract->uncompressCpu(optSO3_->z().data(), optSO3_->z().rows(),
z_.data(), z_.rows());
mfAxes_ = MatrixXf::Zero(3,6);
for(uint32_t k=0; k<6; ++k){
int j = k/2; // which of the rotation columns does this belong to
float sign = (- float(k%2) +0.5f)*2.0f; // sign of the axis
mfAxes_.col(k) = sign*kRw_.col(j);
}
D_KL_= D_KL;
residual_ = residual;
this->update_ = true;
updateLock.unlock();
}
tLog_.toc(2); // total time
tLog_.logCycle();
cout<<"delta rotation kRw_ = \n"<<kRwBefore*kRw_.transpose()<<endl;
cout<<"---------------------------------------------------------------------------"<<endl;
tLog_.printStats();
cout<<" residual="<<residual_<<"\t D_KL="<<D_KL_<<endl;
cout<<"---------------------------------------------------------------------------"<<endl;
fout_<<D_KL_<<" "<<residual_<<endl; fout_.flush();
//// return kRw_;
// {
// boost::mutex::scoped_lock updateLock(this->updateModelMutex);
//// pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr =
//// normalExtract->normalsPc();
//// nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new
//// pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr));
//// this->normalsImg_ = this->normalExtract->normalsImg();
// this->update_ = true;
// }
};
float mmf::OptSO3ApproxGD::conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N)
{
// std::cout << "R before" << std::endl;
// std::cout << R << std::endl;
// Timer t0;
N =0;
float res0 = this->computeAssignment(R,N)/float(N);
if(this->t_ == 0){
// init karcher means with columns of the rotation matrix (takes longer!)
qKarch_ << R.col(0),-R.col(0),R.col(1),-R.col(1),R.col(2),-R.col(2);
// // init karcher means with rotated version of previous karcher means
// qKarch_ = (this->Rprev_*R.transpose())*qKarch_;
}
qKarch_ = karcherMeans(qKarch_, 5.e-5, 10);
// t0.toctic("----- karcher mean");
// compute a rotation matrix from the karcher means (not necessary)
// Matrix3f Rkarch;
// Rkarch.col(0) = qKarch_.col(0);
// Rkarch.col(1) = qKarch_.col(2) - qKarch_.col(2).dot(qKarch_.col(0))
// *qKarch_.col(0);
// Rkarch.col(1) /= Rkarch.col(1).norm();
// Rkarch.col(2) = Rkarch.col(0).cross(Rkarch.col(1));
#ifndef NDEBUG
cout<<"R: "<<endl<<R<<endl;
// cout<<"Rkarch: "<<endl<<Rkarch<<endl<<"det(Rkarch)="<<Rkarch.determinant()<<endl;
std::cout << "qKarch_" << std::endl;
std::cout << qKarch_ << std::endl;
#endif
// t0.tic();
computeSuffcientStatistics();
// t0.toctic("----- sufficient statistics");
return res0; // cost fct value
}
void MiniGL::setViewport (float pfovy, float pznear, float pzfar, const Vector3f &peyepoint, const Vector3f &plookat)
{
fovy = pfovy;
znear = pznear;
zfar = pzfar;
glLoadIdentity ();
gluLookAt (peyepoint [0], peyepoint [1], peyepoint [2], plookat[0], plookat[1], plookat[2], 0, 1, 0);
Matrix4f transformation;
float *lookAtMatrix = transformation.data();
glGetFloatv (GL_MODELVIEW_MATRIX, &lookAtMatrix[0]);
Matrix3f rot;
Vector3f scale;
transformation.transposeInPlace();
rot.row(0) = Vector3f (transformation(0,0), transformation(0,1), transformation(0,2));
rot.row(1) = Vector3f (transformation(1,0), transformation(1,1), transformation(1,2));
rot.row(2) = Vector3f (transformation(2,0), transformation(2,1), transformation(2,2));
scale[0] = rot.col(0).norm();
scale[1] = rot.col(1).norm();
scale[2] = rot.col(2).norm();
m_translation = Vector3f (transformation(3,0), transformation(3,1), transformation(3,2));
rot.col(0) = 1.0f/scale[0] * rot.col(0);
rot.col(1) = 1.0f/scale[1] * rot.col(1);
rot.col(2) = 1.0f/scale[2] * rot.col(2);
m_zoom = scale[0];
m_rotation = Quaternionf(rot);
glLoadIdentity ();
}
void initTable(ColorCloudPtr cloud) {
MatrixXf corners = getTableCornersRansac(cloud);
Vector3f xax = corners.row(1) - corners.row(0);
xax.normalize();
Vector3f yax = corners.row(3) - corners.row(0);
yax.normalize();
Vector3f zax = xax.cross(yax);
float zsgn = (zax(2) > 0) ? 1 : -1;
xax *= - zsgn;
zax *= - zsgn; // so z axis points up
m_axes.col(0) = xax;
m_axes.col(1) = yax;
m_axes.col(2) = zax;
MatrixXf rotCorners = corners * m_axes;
m_mins = rotCorners.colwise().minCoeff();
m_maxes = rotCorners.colwise().maxCoeff();
m_mins(2) = rotCorners(0,2) + LocalConfig::zClipLow;
m_maxes(2) = rotCorners(0,2) + LocalConfig::zClipHigh;
m_transform.setBasis(btMatrix3x3(
xax(0),yax(0),zax(0),
xax(1),yax(1),zax(1),
xax(2),yax(2),zax(2)));
m_transform.setOrigin(btVector3(corners(0,0), corners(0,1), corners(0,2)));
m_poly.points = toROSPoints32(toBulletVectors(corners));
m_inited = true;
}
/* evaluate cost function for a given assignment of npormals to axes */
float mmf::OptSO3ApproxGD::evalCostFunction(Matrix3f& R)
{
float c = 0.0f;
for (uint32_t j=0; j<6; ++j)
{
const float dot = max(-1.0f,min(1.0f,(qKarch_.col(j).transpose() * R.col(j/2))(0)));
if(j%2 ==0){
c += Ns_(j) * acos(dot)* acos(dot);
}else{
c += Ns_(j) * acos(-dot)* acos(-dot);
}
}
return c;
}
int main()
{
Matrix3f m;
m << 1, 2, 3,
4, 5, 6,
7, 8, 9;
std::cout << m << std::endl;
RowVectorXd vec1(3);
vec1 << 1, 2, 3;
std::cout << "vec1 = " << vec1 << std::endl;
RowVectorXd vec2(4);
vec2 << 1, 4, 9, 16;;
std::cout << "vec2 = " << vec2 << std::endl;
RowVectorXd joined(7);
joined << vec1, vec2;
std::cout << "joined = " << joined << std::endl;
MatrixXf matA(2, 2);
matA << 1, 2, 3, 4;
MatrixXf matB(4, 4);
matB << matA, matA/10, matA/10, matA;
std::cout << matB << std::endl;
{
Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;
std::cout << m << std::endl;
}
return 0;
}
/**************************************************************************************************
* Procecure *
* *
* Description: gramSchmidtNormalization *
* Class : ApproachMovementSpace *
**************************************************************************************************/
Matrix3f ApproachMovementSpace::gramSchmidtNormalization3f(Matrix3f matrix)
{
// cout << endl << "Non-Normalized Matrix: " << endl << matrix << endl;
uint matrix_size = matrix.diagonalSize();
for(uint column = 0; column < matrix_size; column += 1)
{
// Step 1: Normalize vector
matrix.col(column) = normalizeVector3f( matrix.col(column) );
// Step 2: Eliminate the projection of this vector
for(uint column_proj = column + 1; column_proj < matrix_size; column_proj += 1)
{
Vector3f vj = matrix.col(column_proj);
matrix.col(column_proj) -= vj.dot( matrix.col(column) ) * matrix.col(column);
}
}
// cout << endl << "Normalized Matrix: " << endl << matrix << endl;
return matrix;
}
// Get traversability of a pose
bool TensorMap::isTraversable(const geometry_msgs::Pose& pose, bool v) {
traversability_timer.start();
const Vector3f position(poseToVector(pose));
const CellKey& key(positionToIndex(position));
const TensorCell& cell((*this)[key]);
// check it is easily traversable
if (isEasilyTraversable(key)) {
traversability_timer.stop();
return true;
}
// if not, check in details
// getting parameters
const TraversabilityParams& params = tensor_map_params->traversability;
const float min_stick_sal = params.min_saliency;
const float max_orientation_angle = params.max_slope;
const int stick_sal_threshold = params.max_points_in_free_cell;
const float min_free_cell_ratio = params.min_free_cell_ratio;
const int nb_max_points = params.max_points_in_bounding_box;
const int nb_min_support = params.min_support_points;
const float length = params.length;
const float width = params.width;
const float height = params.height;
const float l_2 = length/2. + params.inflation;
const float w_2 = width/2. + params.inflation;
const float ground_buffer = params.ground_buffer;
const float buf2 = ground_buffer/2;
const float h_2 = height/2;
const float h0_2 = (height + buf2)/2;
const float h1_2 = (height - buf2)/2;
// check for enough saliency
if (cell.stick_sal<min_stick_sal) {
if (v) {
cout << "Saliency too low: "<<cell.stick_sal<<"/"<<
min_stick_sal<<endl;
}
traversability_timer.stop();
return false;
}
// check orientation is correct
if (cell.angle_to_vertical>max_orientation_angle) {
if (v) {
cout << "Angle too steep: "<<cell.angle_to_vertical<<"/"<<
max_orientation_angle<<endl;
}
traversability_timer.stop();
return false;
}
/*
* check absence of obstacle
*/
// align orientation to surface
Matrix3f alignedAxes;
alignQuaternion(pose.orientation, cell.normal, alignedAxes);
Vector3f x = alignedAxes.col(0);
Vector3f y = alignedAxes.col(1);
Vector3f z = alignedAxes.col(2);
// generate bounding box
Vector3f max_corner = l_2*x.array().abs()+\
w_2*y.array().abs()+\
h0_2*z.array().abs();
Vector3f center = position + h_2*z;
CellKey min_key = positionToIndex(center - max_corner);
CellKey max_key = positionToIndex(center + max_corner);
// shifted up to separate support layer FIXME
center += (buf2/2)*z;
// look through bounding box for possible obstacles
float nb_robot_cells = 0.;
float nb_ok_robot_cells = 0.;
int nb_points_in_cells = 0;
int nb_support_points = 0;
for (int i=min_key.i; i<max_key.i+1; ++i)
for (int j=min_key.j; j<max_key.j+1; ++j)
for (int k=min_key.k; k<max_key.k+1; ++k) {
// check cell is in the robot
TensorCell c = (*this)[CellKey(i, j, k)];
Vector3f rel_pos = c.position - center;
Vector3f rel_pos_aligned = alignedAxes.transpose()*rel_pos;
float rel_x, rel_y, rel_z;
rel_x = rel_pos_aligned(0);
rel_y = rel_pos_aligned(1);
rel_z = rel_pos_aligned(2);
if ((fabs(rel_x)>l_2)||(fabs(rel_y)>w_2)) {
continue;
}
if (fabs(rel_z)>h1_2) {
if (fabs(rel_z+h_2)<buf2) {
nb_support_points += c.nb_points;
}
} else {
// check no obstacle
nb_robot_cells += 1;
if (c.nb_points<=stick_sal_threshold) {
nb_ok_robot_cells += 1;
} else if (c.angle_to_vertical <= max_orientation_angle) {
nb_ok_robot_cells += 1;
nb_points_in_cells += c.nb_points;
}
}
}
// exit if not enough free cells
if ((nb_ok_robot_cells/nb_robot_cells)<min_free_cell_ratio)
{
if (v) {
cout << "not enough free cells: "<<
nb_ok_robot_cells<<"/"<<
min_free_cell_ratio*nb_robot_cells<<" ("<<
nb_robot_cells<<")"<<endl;
}
traversability_timer.stop();
return false;
}
if (nb_points_in_cells>nb_max_points) {
if (v) {
cout << "too many (ground) points in cells: "<<
nb_points_in_cells<<"/"<<nb_max_points<<endl;
}
traversability_timer.stop();
return false;
}
if (nb_support_points<nb_min_support) {
if (v) {
cout << "too few support points in cells: "<<
nb_support_points<<"/"<<nb_min_support<<endl;
}
traversability_timer.stop();
return false;
}
// cell seems valid
if (v) {
cout << "valid" << endl;
}
traversability_timer.stop();
return true;
}
/**************************************************************************************************
* Procecure *
* *
* Description: moveHandRelativeToObject *
* Class : ApproachMovementSpace *
**************************************************************************************************/
Matrix4f ApproachMovementSpace::moveHandRelativeToObject(float theta, float phi, float rho, bool update)
{
RobotPtr hand = eef -> getRobot();
// Don't let the hand collide with the object
if (rho < 10) rho = 10;
// Calculate new XYZ coordinates for the hand
Vector3f new_pos = object -> getGlobalPose().topLeftCorner (3,3) * calculateCarthesianCoordinates(theta, phi, rho);
new_pos += object -> getGlobalPose().topRightCorner(3,1);
Matrix4f new_pose = hand -> getGlobalPose();
Matrix3f orientation;
// Calculate First Vector of the orientation Matrix
// It is colinear with the distance vector between object and hand
orientation.block(0,0,3,1) = object -> getGlobalPose().topRightCorner(3,1) - new_pos;
// Calculate the Remainder two orientation vectors
Matrix3f inertia = object -> getGlobalPose().topLeftCorner(3,3);
inertia *= object -> getInertiaMatrix();
// Select two vectors from the objects inertia matrix
uint vectors_left = 2;
for (uint column = 0; column < 3; column += 1)
{
Vector3f v1 = orientation.block(0,0,3,1);
if( v1.dot( inertia.col(column) ) != 1)
{
orientation.block(0,vectors_left,3,1) = inertia.col(column);
vectors_left -= 1;
if (vectors_left == 0) break;
}
}
// Check if two new vectors were included
if ( vectors_left != 0)
{
cout << endl << "[Error] Function: moveHandRelativeToObject | The two vectors from the object's inertia matrix were not selected.";
exit(-1);
}
// Make the orientation matrix orthonormalized
orientation = gramSchmidtNormalization3f(orientation);
orientation *= offset.topLeftCorner(3,3);
// Check if the new orientation is mirrored or not
// If so, correct it
if ( ( orientation.determinant() - (-1) < 0.01) &&
( orientation.determinant() - (-1) > -0.01) )
{
orientation.col(1) *= -1;
}
// Update new hand pose
new_pose.topLeftCorner (3,3) = orientation;
new_pose.topRightCorner(3,1) = new_pos - orientation * offset.topRightCorner(3,1);
// Set updated pose
if (update) hand -> setGlobalPose(new_pose);
return new_pose;
}
// Compute dense voting
TensorCell& TensorMap::computeDenseVoting(const CellKey& key) {
// timing
dense_voting_timer.start();
// getting parameters
const DenseVotingParams& params = tensor_map_params->dense_voting;
const int maxNbPts = params.max_knn; // max number of neighbors
const float sigma = params.sigma; // tensor voting parameter
const float max_dist = params.max_dist; // maximum radius
const Vector3f& cell_dims = tensor_map_params->cell_dimensions;
//const float cell2 = cell_dims.squaredNorm()/4;
const float obs_dir_offset = params.obs_dir_offset;
const float absolute_saliency_threshold = params.absolute_saliency_threshold;
// position of the point in space
const Vector3f position(indexToPosition(key));
// data in the point cloud
auto eigen_vectors = sparse_map.getDescriptorViewByName("eigen_vectors");
auto eigen_values = sparse_map.getDescriptorViewByName("eigen_values");
auto obsDirection = sparse_map.getDescriptorViewByName\
("observationDirections");
// tensor to vote into
Matrix3f tensor = Matrix3f::Identity();
// getting observation direction from closest point to realign normal
float min_dist2 = numeric_limits<float>::infinity();
int closest_index = -1;
// nearest neighbor search of points for voters
VectorXi indices(maxNbPts);
VectorXf dists2(maxNbPts);
kdTree->knn(position, indices, dists2, maxNbPts, 0, NNSearchF::SORT_RESULTS,
max_dist);
// going through all neighboring points of the center of the cell
int nb_points = 0;
int i;
for (i = 0; i<maxNbPts; ++i) {
if (dists2(i) == numeric_limits<float>::infinity()) { // no match anymore
break;
}
if (dists2(i) == 0.) { // null vote
++nb_points;
continue;
}
int index = indices(i);
// closest point for the point of view
if (dists2(i)<min_dist2) {
min_dist2 = dists2(i);
closest_index = index;
}
// counting the number of points in the cell
const Vector3f voter = sparse_map.features.col(index).head(3);
if (((voter-position).array().abs() <= (cell_dims/2).array()).all()) {
++nb_points;
}
/*
* Full tensor voting
*/
const Vector3f v = position - voter;
Vector3f v_hat;
if (v.isZero()) {
v_hat = v;
} else {
v_hat = v.normalized();
}
// saliencies
float saliencies[3];
saliencies[0] = eigen_values(0, index) - eigen_values(1, index);
saliencies[1] = eigen_values(1, index) - eigen_values(2, index);
saliencies[2] = eigen_values(2, index);
MatrixXf e_vectors(eigen_vectors.col(index));
e_vectors.resize(3, 3);
// normal spaces
Matrix3f normal_spaces[3];
normal_spaces[0] = e_vectors.col(0) * (e_vectors.col(0).transpose());
normal_spaces[1] = normal_spaces[0] + e_vectors.col(1) * (e_vectors.col(1).transpose());
normal_spaces[2] = normal_spaces[1] + e_vectors.col(2) * (e_vectors.col(2).transpose());
// accumulator for all votes of this voter
Matrix3f A = Matrix3f::Zero();
// loop for all 3 votes
for (int d=0; d<3; ++d) {
// normal vector
const Vector3f v_n = normal_spaces[d] * v;
Vector3f v_n_hat;
if (v_n.isZero()) {
v_n_hat = v_n;
} else {
v_n_hat = v_n.normalized();
}
// tangent vector
const Vector3f v_t = v - v_n;
Vector3f v_t_hat;
if (v_t.isZero()) {
v_t_hat = v_t;
} else {
v_t_hat = v_t.normalized();
}
// normal for votee
const float cos_theta = v_hat.dot(v_t_hat);
const float sin_theta = v_hat.dot(v_n_hat);
const float cos_2theta = 2*pow(cos_theta, 2) - 1;
const float sin_2theta = 2*cos_theta*sin_theta;
const Vector3f v_c_hat = cos_2theta * v_n_hat - sin_2theta * v_t_hat;
// weight functions
// using n=4 as in King FIXME
const float w_angle = pow(cos_theta, 2*4);
const float z = v.norm()/sigma;
const float w_dist = pow(z,2)*(pow((z-3),4))/16;
// vote for this dimension
const Matrix3f A_d = w_dist*w_angle*v_c_hat*v_c_hat.transpose() +
w_dist*(normal_spaces[d] - v_n_hat*v_n_hat.transpose());
A += saliencies[d] * A_d;
}
// standard voting FIXME
//tensor += A;
// normalized voting FIXME
tensor += A / (eigen_values(0, index) + absolute_saliency_threshold);
//
/* // old vote
// actual vote
Vector3f e_v = position - voter;
const float r = sqrt(dists2(i));
e_v /= r;
const float stick_sal = eigen_values(0, index) - eigen_values(1, index);
MatrixXf e_vectors(eigen_vectors.col(index));
e_vectors.resize(3, 3);
const Vector3f v_n = e_vectors.col(0);
const Vector3f v_t = v_n.cross(e_v.cross(v_n));
const float cos_theta = e_v.dot(v_t);
const float theta = atan2(e_v.dot(v_n), cos_theta);
// Decoupled weighting profile for angle and distance
// Angle based weighting
const float w_angle = pow(cos_theta,4);
// Distance based weighting
const float z = r/sigma;
const float w_dist = pow(z,2)*(pow((z-3),2))/16;
// stick vote
// could compute sin and cos based on cos_theta and sin_theta FIXME
const Vector3f v_c = v_n*cos(2*theta) - v_t*sin(2*theta);
const float w = w_angle*w_dist;
tensor += stick_sal*w*v_c*v_c.transpose();
//tensor += stick_sal*w*v_c*v_c.transpose()/(eigen_values(0, index) + absolute_saliency_threshold);
*/
}
// analyse resulting tensor
const EigenSolver<Matrix3f> solver(tensor);
Vector3f eigenVals = solver.eigenvalues().real();
Matrix3f eigenVectors = solver.eigenvectors().real();
sortTensor(eigenVals, eigenVectors);
float stick_sal = eigenVals(0)-eigenVals(1);
float s = eigenVals(0)-eigenVals(1);
//float p = eigenVals(1)-eigenVals(2);
//float b = eigenVals(2);
float Z = eigenVals(0);
//cout << "Saliency distribution: (" << s/Z << ", " << p/Z << ", " << b/Z << "); abs_sal=" << Z << "; p_plane=" << s/(Z+absolute_saliency_threshold) << " (" << absolute_saliency_threshold << ")" << endl;
Vector3f normal;
if (closest_index == -1) {
// no point at all
normal = {0., 0., 0.};
stick_sal = 0.;
} else {
// flip normal according to closest point
const Vector3f obsDir = obsDirection.col(closest_index)+
Vector3f(0, 0, obs_dir_offset);
const Vector3f v_n = eigenVectors.col(0);
if (obsDir.dot(v_n)<0.) {
normal = -v_n;
} else {
normal = v_n;
}
}
dense_map.insert({key, TensorCell(position, stick_sal, normal, nb_points,
UNKNOWN, s/(Z+absolute_saliency_threshold))
});
// stopping timer
dense_voting_timer.stop();
return dense_map.at(key);
}
// Get faster traversability
bool TensorMap::isEasilyTraversable(const CellKey& key) {
TensorCell& cell((*this)[key]);
// check cache
if (cell.traversability!=UNKNOWN) {
return (cell.traversability==OK);
}
// compute it
const bool v = false;
// getting parameters
const TraversabilityParams& params = tensor_map_params->traversability;
const float min_stick_sal = params.min_saliency;
const float max_orientation_angle = params.max_slope;
const int stick_sal_threshold = params.max_points_in_free_cell;
const float min_free_cell_ratio = params.min_free_cell_ratio;
const int nb_max_points = params.max_points_in_bounding_box;
const int nb_min_support = params.min_support_points;
const float diameter = params.diameter;
const float d_2 = diameter/2. + params.inflation;
const float r2 = d_2*d_2;
const float height = params.height;
const float ground_buffer = params.ground_buffer;
const float buf2 = ground_buffer/2;
const float h_2 = height/2;
const float h0_2 = (height + buf2)/2;
const float h1_2 = (height - buf2)/2;
// check for enough saliency
if (cell.stick_sal<min_stick_sal) {
if (v) {
cout << "Saliency too low: "<<cell.stick_sal<<"/"<<
min_stick_sal<<endl;
}
cell.traversability = NOT_SALIENCY;
return false;
}
// check orientation is correct
if (cell.angle_to_vertical>max_orientation_angle) {
if (v) {
cout << "Angle too steep: "<<cell.angle_to_vertical<<"/"<<
max_orientation_angle<<endl;
}
cell.traversability = NOT_ANGLE;
return false;
}
/*
* check absence of obstacle
*/
// align orientation to surface
Matrix3f alignedAxes;
geometry_msgs::Quaternion quat;
quat.x = quat.y = quat.z = 0;
quat.w = 1;
alignQuaternion(quat, cell.normal, alignedAxes);
Vector3f x = alignedAxes.col(0);
Vector3f y = alignedAxes.col(1);
Vector3f z = alignedAxes.col(2);
// generate bounding box
Vector3f max_corner = d_2*x.array().abs()+\
d_2*y.array().abs()+\
h0_2*z.array().abs();
Vector3f center = cell.position + h_2*z;
CellKey min_key = positionToIndex(center - max_corner);
CellKey max_key = positionToIndex(center + max_corner);
// shifted up to separate support layer FIXME
center += buf2/2*z;
// look through bounding box for possible obstacles
float nb_robot_cells = 0.;
float nb_ok_robot_cells = 0.;
int nb_points_in_cells = 0;
int nb_support_points = 0;
for (int i=min_key.i; i<max_key.i+1; ++i)
for (int j=min_key.j; j<max_key.j+1; ++j)
for (int k=min_key.k; k<max_key.k+1; ++k) {
// check cell is in the robot
TensorCell c = (*this)[CellKey(i, j, k)];
Vector3f rel_pos = c.position - center;
Vector3f rel_pos_aligned = alignedAxes.transpose()*rel_pos;
float rel_x, rel_y, rel_z;
rel_x = rel_pos_aligned(0);
rel_y = rel_pos_aligned(1);
rel_z = rel_pos_aligned(2);
if (pow(rel_x, 2)+pow(rel_y, 2)>r2) {
continue;
}
if (fabs(rel_z)>h1_2) {
if (fabs(rel_z+h_2)<buf2) {
nb_support_points += c.nb_points;
}
} else {
// check no obstacle
nb_robot_cells += 1;
if (c.nb_points<=stick_sal_threshold) {
nb_ok_robot_cells += 1;
} else if (c.angle_to_vertical <= max_orientation_angle) {
nb_ok_robot_cells += 1;
nb_points_in_cells += c.nb_points;
}
}
}
// exit if not enough free cells
if ((nb_ok_robot_cells/nb_robot_cells)<min_free_cell_ratio)
{
if (v) {
cout << "not enough free cells: "<<
nb_ok_robot_cells<<"/"<<
min_free_cell_ratio*nb_robot_cells<<" ("<<
nb_robot_cells<<")"<<endl;
}
cell.traversability = NOT_OBSTACLE;
return false;
}
if (nb_points_in_cells>nb_max_points) {
if (v) {
cout << "too many (ground) points in cells: "<<
nb_points_in_cells<<"/"<<nb_max_points<<endl;
}
cell.traversability = NOT_IN_GROUND;
return false;
}
if (nb_support_points<nb_min_support) {
if (v) {
cout << "too few support points in cells: "<<
nb_support_points<<"/"<<nb_min_support<<endl;
}
cell.traversability = NOT_IN_AIR;
return false;
}
// cell seems valid
if (v) {
cout << "valid" << endl;
}
cell.traversability = OK;
return true;
}
int main(int argc, char** argv) {
MatrixXf m(3,3);
m << 1,2,3,4,5,6,7,8,9;
Vector3f v = m.col(0);
Vector3f v2 = m.col(1);
Vector3f v3 = m.col(2);
cout<<"Mat = "<<m<<endl;
cout<<"vector = " << v<<endl;
MatrixXf n = (m.rowwise() -= v);
cout<<"Mat2 = "<<n<<endl;
Matrix3f r;
r.col(0) = v;
r.col(1) = v2;
r.col(2) = v3;
cout<<"Mat3 = "<<r<<endl;
r *= 2;
cout<<"Mat3 = "<<r<<endl;
cout<<"Mat3 = "<<m<<endl;
float arr[] = {1,2,3};
vector<float> w(arr, arr + sizeof(arr) / sizeof(float));
for(int i = 0; i < w.size(); i+=1)
cout<<w[i]<<"\t";
cout<<endl<<"---------------"<<endl;
/** Inverting a singular matrix. **/
m << 1,0,0,1,0,0,1,0,0;
cout<<" Should segfault/ get garbage: "<<m.determinant()<<endl;
/** Affine matrix in Eigen. */
MatrixXf ml(4,4);
ml << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
Affine3f t;
Vector3f d(10,20,30);
t.linear() = ml.transpose().block(0,0,3,3);
t.translation() = ml.block(0,3,3,1);
cout<<"Affine: "<<endl<<t.affine()<<endl;
Vector3f tt(1,2,3);
t.translation() += tt;
cout<<"Affine after translation: "<<endl<<t.affine()<<endl;
/** Blocks. */
MatrixXf matr(3,2);
matr << 1,2,3,4,5,6;
MatrixXf combo(4,2);
combo <<matr, MatrixXf::Ones(1,matr.cols());
cout<<"Matr = "<<matr<<endl;
cout<<"Comb = "<<combo<<endl;
MatrixXf rrr = combo.block(0,0,3,2);
cout<<"rrr = "<<rrr<<endl;
/** Filling up a matrix*/
std::cout<<"---------------------------------------"<<std::endl;
MatrixXf matF(5,3);
Vector3f vF(1,.3,3);
Vector3f vF1(.123,.2,.3);
Vector3f vF2(33.4,23.3,12.07);
Vector3f vF3(0.54,8.96,14.3);
Vector3f vF4(8.9,0.34,32.2);
matF.row(0) = vF;
matF.row(1) = vF1;
matF.row(2) = vF2;
matF.row(3) = vF3;
matF.row(4) = vF4;
RowVector3f means = matF.colwise().sum()/5;
MatrixXf centered = (matF.rowwise() - means);
cout<<"Matrix Fill = \n"<<matF<<std::endl;
cout<<"Means = \n"<<means<<std::endl;
cout<<"Centered = \n"<<centered<<std::endl;
Eigen::JacobiSVD<MatrixXf> svd(centered / sqrt(5), ComputeFullV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
MatrixXf V = svd.matrixV();
Vector3f f1 = V.col(0);
Vector3f f2 = V.col(1);
Vector3f f3 = V.col(2);
cout << "f1(0)" << f1(0) << endl;
VectorXf faa(V.rows());
faa.setZero();
//for (int i= 0; i < V.rows(); i++)
V.col(2) = faa;
cout << "V " << V<< endl;
cout << "<v1,v2>" << f1.dot(f2) << endl;
cout << "<v1,v3>" << f1.dot(f3) << endl;
cout << "<v3,v2>" << f3.dot(f2) << endl;
std::cout<<"---------------------------------------"<<std::endl;
Vector3f o3(1,2,3), o2(4,5,6);
cout << "outer? : "<< o3.cwiseProduct(o2) <<endl;
MatrixXd mxx(3,3);
mxx << 1,0,0,0,2,0,0,0,3;
cout << "mxx : "<< endl<<mxx <<endl;
vector<double> v31(3);
VectorXd::Map(&v31[0], 3) = mxx.row(0);
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout<< v31[i]<< " " <<endl;
cout<<endl;
v31[1] = 100;
cout << "v: "<<endl;
for(int i=0; i < 3; i++)
cout << v31[i]<< " " <<endl;
cout<<endl;
cout << " mxx : "<<endl <<mxx<<endl;
Matrix3d tmat = Matrix3d::Identity();
tmat(0,0) = 0;
tmat(1,1) = 10;
tmat(1,2) = 20;
cout << "tmat: " << tmat<<endl;
cout << "------------\n";
for (unsigned i=0; i < tmat.rows(); i++) {
double sum = tmat.row(i).sum() + numeric_limits<double>::min();
tmat.row(i) /= sum;
}
cout << tmat << endl;
MatrixXd src(4,3);
src<< 0,0,0,1,1,1,2,2,2,3,3,3;
MatrixXd target(1,3);
target<< 1,1,1;
cout <<"cloud err: "<< (src.rowwise() -target.row(0)).rowwise().squaredNorm().transpose()<<endl;
return 0;
}