inline
unsigned int operator()(const TPoint3D &aNormal, const Z2i::Point &aPoint, const double h) const {
TPoint3D l;
Z3i::RealPoint posL (aPoint[0], aPoint[1], h);
l = -posL+myLightSourcePosition;
l /= l.norm();
double lambertianIntensity = std::max(aNormal.dot(l), 0.0);
double alpha = acos(((l+Z3i::RealPoint(0,0,1.0))/2.0).dot(aNormal/aNormal.norm()));
double specularIntensity = exp(-alpha*alpha/(2.0*mySigma));
double resu = myKld*lambertianIntensity+myKls*specularIntensity;
resu = std::max(resu, 0.0);
resu = std::min(resu, 1.0);
return resu*std::numeric_limits<typename TImage2D::Value>::max();
}
inline
unsigned int operator()(const TPoint3D &aNormal, const Z2i::Point &aPoint, const double h) const
{
TPoint3D l;
Z3i::RealPoint posL (aPoint[0], aPoint[1], h);
l = -posL+myLightSourcePosition;
l /= l.norm();
int intensity = aNormal.dot(l)*std::numeric_limits<typename TImage2D::Value>::max();
return intensity>0? intensity:0;
}
/*---------------------------------------------------------------
sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DRotVec::sphericalCoordinates(
const TPoint3D &point,
double &out_range,
double &out_yaw,
double &out_pitch ) const
{
// Pass to coordinates as seen from this 6D pose:
TPoint3D local;
this->inverseComposePoint(point.x,point.y,point.z, local.x,local.y,local.z);
// Range:
out_range = local.norm();
// Yaw:
if (local.y!=0 || local.x!=0)
out_yaw = atan2(local.y,local.x);
else out_yaw = 0;
// Pitch:
if (out_range!=0)
out_pitch = -asin( local.z / out_range );
else out_pitch = 0;
}
void insertRandomPoints_screw(
const size_t N,
opengl::CPointCloudPtr &gl,
const TPoint3D &p_start,
const TPoint3D &p_end)
{
TPoint3D d = p_end-p_start;
d*= 1.0/N;
TPoint3D up(0,0,1);
TPoint3D lat;
mrpt::math::crossProduct3D(d,up, lat);
lat*=1.0/lat.norm();
TPoint3D p = p_start;
for (size_t i=0;i<N;i++)
{
const double ang = i*0.01;
TPoint3D pp = p + up * 30*cos(ang) + lat * 30*sin(ang);
gl->insertPoint(pp.x,pp.y,pp.z);
p+=d;
}
}
void openbeam::internal::computeOrientationFromTwoPoints(
const TPoint3D &t1,
const TPoint3D &t2,
const num_t rotation_around_x,
TRotation3D &out_R)
{
// Vector: 1 -> 2
const TPoint3D p12 = TPoint3D(t2.coords[0]-t1.coords[0], t2.coords[1]-t1.coords[1], t2.coords[2]-t1.coords[2]);
const num_t len = p12.norm();
TMatrix33 R;
if (len>0)
{
// Normal axis:
const num_t len_inv = 1/len;
num_t X[3],Y[3],Z[3]; // Coordinates of the NEW axes wrt GLOBAL coordinates frame
// New X:
X[0]=p12.coords[0]*len_inv;
X[1]=p12.coords[1]*len_inv;
X[2]=p12.coords[2]*len_inv;
// New Z: orthogonal to X and with Zy=0:
if (X[2]==0)
{
Z[0]=0;
Z[1]=0;
Z[2]=1;
}
else
{
if (X[0]!=0 || X[1]!=0)
{
Z[0] = -X[0];
Z[1] = -X[1];
Z[2] = (X[0]*X[0]+X[1]*X[1])/X[2];
const num_t norm_fact = num_t(1)/std::sqrt(square(Z[0])+square(Z[1])+square(Z[2]));
Z[0]*=norm_fact; Z[1]*=norm_fact; Z[2]*=norm_fact;
}
else
{
Z[0] = -1;
Z[1] = 0;
Z[2] = 0;
}
}
// New Y: cross product:
Y[0] = Z[1]*X[2] - Z[2]*X[1];
Y[1] = Z[2]*X[0] - Z[0]*X[2];
Y[2] = Z[0]*X[1] - Z[1]*X[0];
// Now, rotate around the new X axis "m_design_rotation_around_linear_axis" radians
R(0,0)=X[0]; R(0,1)=Y[0]; R(0,2)=Z[0]; // X' is not affected.
R(1,0)=X[1]; R(1,1)=Y[1]; R(1,2)=Z[1];
R(2,0)=X[2]; R(2,1)=Y[2]; R(2,2)=Z[2];
}
else
{
// For elements without any length, use global coordinates:
R.setIdentity();
}
if (rotation_around_x!=0)
{
// Y'' = sin()*Z' + cos()*Y'
// Z'' = cos()*Z' - sin()*Y'
//
Eigen::Matrix<num_t,2,2> axis_rot;
const num_t c = cos(rotation_around_x);
const num_t s = sin(rotation_around_x);
axis_rot(0,0)=c; axis_rot(0,1)=-s;
axis_rot(1,0)=s; axis_rot(1,1)=c;
R.block(1,0,2,3) = axis_rot * R.block(1,0,2,3);
}
out_R.setRot(R);
}
LambertianShadindFunctor(const TPoint3D &aLightSourceDir ):
myLightSourceDirection(aLightSourceDir/aLightSourceDir.norm()){}
inline
unsigned int operator()(const TPoint3D &aNormal) const {
double lambertianIntensity = std::max(aNormal.dot(myLightSourceDirection), 0.0);
double alpha = acos(((myLightSourceDirection+Z3i::RealPoint(0,0,1.0))/2.0).dot(aNormal/aNormal.norm()));
double specularIntensity = exp(-alpha*alpha/(2.0*mySigma));
double resu = myKld*lambertianIntensity+myKls*specularIntensity;
resu = std::max(resu, 0.0);
resu = std::min(resu, 1.0);
return resu*std::numeric_limits<typename TImage2D::Value>::max();
}
SpecularNayarShadindFunctor(const TPoint3D &lightSourceDirection, const double kld,
const double kls, const double sigma ):
myLightSourceDirection(lightSourceDirection/lightSourceDirection.norm()),
myKld(kld), myKls(kls), mySigma(sigma){}
void CPointCloudFilterByDistance::filter(
/** [in,out] The input pointcloud, which will be modified upon return after
filtering. */
mrpt::maps::CPointsMap* pc,
/** [in] The timestamp of the input pointcloud */
const mrpt::system::TTimeStamp pc_timestamp,
/** [in] If nullptr, the PC is assumed to be given in global coordinates.
Otherwise, it will be transformed from local coordinates to global using
this transformation. */
const mrpt::poses::CPose3D& cur_pc_pose,
/** [in,out] additional in/out parameters */
TExtraFilterParams* params)
{
using namespace mrpt::poses;
using namespace mrpt::math;
using mrpt::math::square;
MRPT_START;
ASSERT_(pc_timestamp != INVALID_TIMESTAMP);
ASSERT_(pc != nullptr);
CSimplePointsMap::Ptr original_pc =
mrpt::make_aligned_shared<CSimplePointsMap>();
original_pc->copyFrom(*pc);
// 1) Filter:
// ---------------------
const size_t N = pc->size();
std::vector<bool> deletion_mask;
deletion_mask.assign(N, false);
size_t del_count = 0;
// get reference, previous PC:
ASSERT_(options.previous_keyframes >= 1);
bool can_do_filter = true;
std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
{
auto it = m_last_frames.rbegin();
for (int i = 0;
i < options.previous_keyframes && it != m_last_frames.rend();
++i, ++it)
{
prev_pc.push_back(&it->second);
}
}
if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
{
can_do_filter = false;
}
else
{
for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
{
if (mrpt::system::timeDifference(
m_last_frames.rbegin()->first, pc_timestamp) >
options.too_old_seconds)
{
can_do_filter = false; // A required keyframe is too old
break;
}
}
}
if (can_do_filter)
{
// Reference poses of each PC:
// Previous: prev_pc.pose
mrpt::aligned_containers<CPose3D>::vector_t rel_poses;
for (int k = 0; k < options.previous_keyframes; ++k)
{
const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
rel_poses.push_back(rel_pose);
}
// The idea is that we can now find matches between pt{i} in time_{k},
// composed with rel_pose
// with the local points in time_{k-1}.
std::vector<TPoint3D> pt_km1(options.previous_keyframes);
for (size_t i = 0; i < N; i++)
{
// get i-th point in time=k:
TPoint3Df ptf_k;
pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
const TPoint3D pt_k = TPoint3D(ptf_k);
// Point, referred to time=k-1 frame of reference
for (int k = 0; k < options.previous_keyframes; ++k)
rel_poses[k].composePoint(pt_k, pt_km1[k]);
// Look for neighbors in "time=k"
std::vector<TPoint3D> neig_k;
std::vector<float> neig_sq_dist_k;
pc->kdTreeNClosestPoint3D(
pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
// Look for neighbors in "time=k-i"
std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
std::vector<float> neig_sq_dist_kmi(
options.previous_keyframes, std::numeric_limits<float>::max());
for (int k = 0; k < options.previous_keyframes; ++k)
{
for (int prev_tim_idx = 0;
prev_tim_idx < options.previous_keyframes; prev_tim_idx++)
{
if (prev_pc[prev_tim_idx]->pc->size() > 0)
{
prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(
pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx],
neig_sq_dist_kmi[prev_tim_idx]);
}
}
}
// Rule:
// we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
const double max_allowed_dist_sq = square(
options.min_dist + options.angle_tolerance * pt_k.norm());
bool ok_total = true;
const bool ok_t =
neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
ok_total = ok_total && ok_t;
for (int k = 0; k < options.previous_keyframes; ++k)
{
const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
ok_total = ok_total && ok_tm1;
}
// Delete?
const bool do_delete = !(ok_total);
deletion_mask[i] = do_delete;
if (do_delete) del_count++;
}
// Remove points:
if ((params == nullptr || params->do_not_delete == false) && N > 0 &&
del_count / double(N) <
options.max_deletion_ratio // If we are deleting too many
// points, it may be that the filter
// is plainly wrong
)
{
pc->applyDeletionMask(deletion_mask);
}
} // we can do filter
if (params != nullptr && params->out_deletion_mask != nullptr)
{
*params->out_deletion_mask = deletion_mask;
}
// 2) Add PC to list
// ---------------------
{
FrameInfo fi;
fi.pc = original_pc;
fi.pose = cur_pc_pose;
m_last_frames[pc_timestamp] = fi;
}
// 3) Remove too-old PCs.
// ---------------------
for (auto it = m_last_frames.begin(); it != m_last_frames.end();)
{
if (mrpt::system::timeDifference(it->first, pc_timestamp) >
options.too_old_seconds)
{
it = m_last_frames.erase(it);
}
else
{
++it;
}
}
MRPT_END;
}
/*---------------------------------------------------------------
sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DQuat::sphericalCoordinates(
const TPoint3D &point,
double &out_range,
double &out_yaw,
double &out_pitch,
mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint,
mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose
) const
{
const bool comp_jacobs = out_jacob_dryp_dpoint!=NULL || out_jacob_dryp_dpose!=NULL;
// Pass to coordinates as seen from this 6D pose:
CMatrixFixedNumeric<double,3,3> jacob_dinv_dpoint, *ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : NULL;
CMatrixFixedNumeric<double,3,7> jacob_dinv_dpose, *ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : NULL;
TPoint3D local;
this->inverseComposePoint(point.x,point.y,point.z, local.x,local.y,local.z,ptr_ja1,ptr_ja2);
// Range:
out_range = local.norm();
// Yaw:
if (local.y!=0 || local.x!=0)
out_yaw = atan2(local.y,local.x);
else out_yaw = 0;
// Pitch:
if (out_range!=0)
out_pitch = -asin( local.z / out_range );
else out_pitch = 0;
// Jacobians are:
// dryp_dpoint = dryp_dlocalpoint * dinv_dpoint
// dryp_dpose = dryp_dlocalpoint * dinv_dpose
if (comp_jacobs)
{
if (out_range==0)
THROW_EXCEPTION("Jacobians are undefined for range=0")
/* MATLAB:
syms H h_range h_yaw h_pitch real;
syms xi_ yi_ zi_ real;
h_range = sqrt(xi_^2+yi_^2+zi_^2);
h_yaw = atan(yi_/xi_);
% h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
H=[ h_range ; h_yaw ; h_pitch ];
jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
*/
const double _r = 1.0/out_range;
const double x2 = square(local.x);
const double y2 = square(local.y);
const double t2 = std::sqrt(x2 + y2);
const double _K = 1.0/(t2*square(out_range));
double vals[3*3]= {
local.x*_r, local.y*_r, local.z*_r,
-local.y/(x2*(y2/x2 + 1)), 1.0/(local.x*(y2/x2 + 1)), 0,
(local.x*local.z) * _K, (local.y*local.z) * _K, -t2/square(out_range)
};
const CMatrixDouble33 dryp_dlocalpoint(vals);
if (out_jacob_dryp_dpoint)
out_jacob_dryp_dpoint->multiply(dryp_dlocalpoint,jacob_dinv_dpoint);
if (out_jacob_dryp_dpose)
out_jacob_dryp_dpose->multiply(dryp_dlocalpoint,jacob_dinv_dpose);
}
}