bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
raw, Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
raw.data());
Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
map_of_const_rxso3.quaternion().coeffs().eval());
Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2;
map_of_rxso3.setQuaternion(quat);
SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
raw.data());
SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
quat.coeffs().data());
Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
map_of_rxso3.quaternion().coeffs().eval());
RxSO3Type const const_so3(quat);
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
}
RxSO3Type so3(quat);
for (int i = 0; i < 4; ++i) {
so3.data()[i] = raw[i];
}
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
}
for (int i = 0; i < 10; ++i) {
Matrix3<Scalar> M = Matrix3<Scalar>::Random();
for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
Matrix3<Scalar> R = makeRotationMatrix(M);
Matrix3<Scalar> sR = scale * R;
SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
"isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
Matrix3<Scalar> sR_cols_swapped;
sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
"isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
}
}
return passed;
}
void cv2eigen( const Mat& src,
Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst )
{
CV_DbgAssert(src.rows == _rows && src.cols == _cols);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
Mat _dst(src.cols, src.rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
if( src.type() == _dst.type() )
transpose(src, _dst);
else if( src.cols == src.rows )
{
src.convertTo(_dst, _dst.type());
transpose(_dst, _dst);
}
else
Mat(src.t()).convertTo(_dst, _dst.type());
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
else
{
Mat _dst(src.rows, src.cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
src.convertTo(_dst, _dst.type());
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
}
template<typename _Tp> static inline
void cv2eigen( const Mat& src,
Eigen::Matrix<_Tp, Eigen::Dynamic, Eigen::Dynamic>& dst )
{
dst.resize(src.rows, src.cols);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
const Mat _dst(src.cols, src.rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
if( src.type() == _dst.type() )
transpose(src, _dst);
else if( src.cols == src.rows )
{
src.convertTo(_dst, _dst.type());
transpose(_dst, _dst);
}
else
Mat(src.t()).convertTo(_dst, _dst.type());
}
else
{
const Mat _dst(src.rows, src.cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
src.convertTo(_dst, _dst.type());
}
}
IGL_INLINE void igl::create_vector_vbo(
const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> & V,
GLuint & V_vbo_id)
{
//// Expects that input is list of 3D vectors along rows
//assert(V.cols() == 3);
// Generate Buffers
glGenBuffers(1,&V_vbo_id);
// Bind Buffers
glBindBuffer(GL_ARRAY_BUFFER,V_vbo_id);
// Copy data to buffers
// We expect a matrix with each vertex position on a row, we then want to
// pass this data to OpenGL reading across rows (row-major)
if(V.Options & Eigen::RowMajor)
{
glBufferData(
GL_ARRAY_BUFFER,
sizeof(T)*V.size(),
V.data(),
GL_STATIC_DRAW);
}else
{
// Create temporary copy of transpose
Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> VT = V.transpose();
// If its column major then we need to temporarily store a transpose
glBufferData(
GL_ARRAY_BUFFER,
sizeof(T)*V.size(),
VT.data(),
GL_STATIC_DRAW);
}
// bind with 0, so, switch back to normal pointer operation
glBindBuffer(GL_ARRAY_BUFFER, 0);
}
bool ReprojectionErrorFixed::Evaluate(double const* const* args,
double* residuals,
double** jac) const
{
double newPoint[3];
double rotationVec[3];
const double * const landmark = args[0];
Vector3d X(landmark);
X = RcamBase*(RbaseOrig*X + PbaseOrig) + PcamBase;
Vector2d point;
camera->projectPoint(X, point);
residuals[0] = point[0] - u;
residuals[1] = point[1] - v;
if (jac)
{
Eigen::Matrix<double, 2, 3> J;
camera->projectionJacobian(X, J);
// dp / dX
Eigen::Matrix<double, 2, 3, RowMajor> dpdX = J * RcamBase * RbaseOrig;
copy(dpdX.data(), dpdX.data() + 6, jac[0]);
}
return true;
}
// dynVCLVec(viennacl::vector_range<viennacl::vector_base<T> > vec, int ctx_id){
// viennacl::context ctx;
//
// // explicitly pull context for thread safe forking
// ctx = viennacl::context(viennacl::ocl::get_context(static_cast<long>(ctx_id)));
//
// A.switch_memory_context(ctx);
// A = vec;
//
// size = A.size();
// begin = 1;
// last = size;
// ptr = &A;
// viennacl::range temp_r(0, size);
// r = temp_r;
// }
dynVCLVec(SEXP A_, int ctx_id) {
#ifdef UNDEF
Eigen::Matrix<T, Eigen::Dynamic, 1> Am;
Am = Rcpp::as<Eigen::Matrix<T, Eigen::Dynamic, 1> >(A_);
int K = Am.size();
viennacl::context ctx;
// explicitly pull context for thread safe forking
ctx = viennacl::context(viennacl::ocl::get_context(static_cast<long>(ctx_id)));
// std::cout << "about to initialize" << std::endl;
viennacl::vector_base<T> A = viennacl::vector_base<T>(K, ctx);
// std::cout << "initialized vector" << std::endl;
viennacl::fast_copy(Am.data(), Am.data() + Am.size(), A.begin());
// viennacl::fast_copy(Am.begin(), Am.end(), A.begin());
// std::cout << "copied" << std::endl;
size = A.size();
begin = 1;
last = size;
// ptr = &A;
viennacl::range temp_r(0, size);
r = temp_r;
shared = false;
shared_type = 0;
shptr = std::make_shared<viennacl::vector_base<T> >(A);
#endif
}
void SetTwistHandler::handleSimulation(){
// called when the main script calls: simHandleModule
if(!_initialized){
_initialize();
}
Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
(simFloat)_twistCommands.twist.linear.y,
(simFloat)_twistCommands.twist.linear.z);
Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
(simFloat)_twistCommands.twist.angular.y,
(simFloat)_twistCommands.twist.angular.z);
// Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
linVelocity = orientation*linVelocity;
angVelocity = orientation*angVelocity;
} else {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
ConsoleHandler::printInConsole(ss);
}
simResetDynamicObject(_associatedObjectID);
//Set object velocity
if (_isStatic){
Eigen::Matrix<simFloat, 3, 1> position;
simGetObjectPosition(_associatedObjectID, -1, position.data());
const simFloat timeStep = simGetSimulationTimeStep();
position = position + timeStep*linVelocity;
simSetObjectPosition(_associatedObjectID, -1, position.data());
const simFloat angle = timeStep*angVelocity.norm();
if(angle > 1e-6){
orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
}
simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
} else {
// Apply the linear velocity to the object
if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
ConsoleHandler::printInConsole(ss);
}
// Apply the angular velocity to the object
if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
ConsoleHandler::printInConsole(ss);
}
}
}
void GetTwistHandler::handleSimulation(){
// called when the main script calls: simHandleModule
if(!_initialized){
_initialize();
}
ros::Time now = ros::Time::now();
const simFloat currentSimulationTime = simGetSimulationTime();
if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){
Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
Eigen::Matrix<simFloat, 3, 1> linVelocity;
Eigen::Matrix<simFloat, 3, 1> angVelocity;
bool error = false;
// Get object velocity. If the object is not static simGetVelocity is more accurate.
if (_isStatic){
error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
} else {
error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
}
// Get object orientation
error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;
if(!error){
linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame
// Fill the status msg
geometry_msgs::TwistStamped msg;
msg.twist.linear.x = linVelocity[0];
msg.twist.linear.y = linVelocity[1];
msg.twist.linear.z = linVelocity[2];
msg.twist.angular.x = angVelocity[0];
msg.twist.angular.y = angVelocity[1];
msg.twist.angular.z = angVelocity[2];
msg.header.stamp = now;
_pub.publish(msg);
_lastPublishedObjTwistTime = currentSimulationTime;
} else {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
ConsoleHandler::printInConsole(ss);
}
}
}
bool ReprojectionErrorStereo::Evaluate(double const* const* args,
double* residuals,
double** jac) const
{
Vector3d rot(args[2]);
Matrix3d RbaseOrig = rotationMatrix<double>(-rot);
Vector3d Pob(args[1]);
Vector3d X(args[0]);
X = RcamBase * (RbaseOrig * (X - Pob)) + PcamBase;
Vector2d point;
camera->projectPoint(X, point);
residuals[0] = point[0] - u;
residuals[1] = point[1] - v;
if (jac)
{
Eigen::Matrix<double, 2, 3> J;
camera->projectionJacobian(X, J);
Matrix3d Rco = RcamBase * RbaseOrig;
// dp / dX
Eigen::Matrix<double, 2, 3, RowMajor> dpdX = J * Rco;
copy(dpdX.data(), dpdX.data() + 6, jac[0]);
// dp / dxi
Eigen::Matrix<double, 3, 3> LxiInv;
double theta = rot.norm();
if ( theta != 0)
{
Matrix3d uhat = hat<double>(rot / theta);
LxiInv = Matrix3d::Identity() +
theta/2*sinc(theta/2)*uhat +
(1 - sinc(theta))*uhat*uhat;
}
else
{
LxiInv = Matrix3d::Identity();
}
Eigen::Matrix<double, 2, 3, RowMajor> dpdxi2; // = (Eigen::Matrix<double, 2, 3, RowMajor> *) jac[2];
dpdX *= -1;
dpdxi2 = J*hat(X)*Rco*LxiInv;
copy(dpdX.data(), dpdX.data() + 6, jac[1]);
copy(dpdxi2.data(), dpdxi2.data() + 6, jac[2]);
}
return true;
}
void NNUtilities::normalizeContrast(OutType* output, const Vector2i size, const float percent)
{
Eigen::Map<Eigen::Matrix<OutType, Eigen::Dynamic, Eigen::Dynamic>> patch(output, size.x(), size.y());
Eigen::Matrix<OutType, Eigen::Dynamic, 1> sorted = Eigen::Map<Eigen::Matrix<OutType, Eigen::Dynamic, 1>>(patch.data(), size.x() * size.y());
std::sort(sorted.data(), sorted.data() + sorted.size());
OutType min = sorted(static_cast<int>((sorted.size() - 1) * percent));
OutType max = sorted(static_cast<int>((sorted.size() - 1) * (1.f - percent)));
if(max == 0)
patch.setConstant(0);
else
patch.array() = ((patch.array().max(min).min(max) - min).template cast<float>() * 255.f / (static_cast<float>(max - min))).template cast<OutType>();
}
// Matx case
template<typename _Tp, int _rows, int _cols, int _options, int _maxRows, int _maxCols> static inline
void eigen2cv( const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src,
Matx<_Tp, _rows, _cols>& dst )
{
if( !(src.Flags & Eigen::RowMajorBit) )
{
dst = Matx<_Tp, _cols, _rows>(static_cast<const _Tp*>(src.data())).t();
}
else
{
dst = Matx<_Tp, _rows, _cols>(static_cast<const _Tp*>(src.data()));
}
}
void grouped_vocabulary_tree<Point, K>::set_input_cloud(CloudPtrT& new_cloud, vector<pair<int, int> >& indices)
{
// to begin with, we might have to remove nan points
Eigen::Matrix<float, super::rows, 1> p;
CloudPtrT temp_cloud(new CloudT);
temp_cloud->reserve(new_cloud->size());
vector<pair<int, int> > temp_indices;
temp_indices.reserve(indices.size());
for (size_t i = 0; i < new_cloud->size(); ++i) {
p = eig(new_cloud->at(i));
if (std::find_if(p.data(), p.data()+super::rows, [] (float f) {
return std::isnan(f) || std::isinf(f);
}) == p.data()+super::rows) {
temp_cloud->push_back(new_cloud->at(i));
temp_indices.push_back(indices[i]);
}
}
cout << "Temp cloud size: " << temp_cloud->size() << endl;
cout << "Indices size: " << temp_indices.size() << endl;
//std::sort(temp_indices.begin(), temp_indices.end(), [](const pair<int, int>& p1, const pair<int, int>& p2) {
// return p1.first < p2.first && p1.second < p2.second;
//});
vector<int> new_indices(temp_indices.size());
pair<int, int> previous_p = make_pair(-1, -1);
int index_group_ind = -1;
int counter = 0;
for (const pair<int, int>& p : temp_indices) {
// everything with this index pair should have the same label, assume ordered
if (p != previous_p) {
++index_group_ind;
index_group[index_group_ind] = p.first; // these two really should be vectors instead
group_subgroup[index_group_ind] = p;
previous_p = p;
}
new_indices[counter] = index_group_ind;
++counter;
}
nbr_points = counter;
nbr_groups = index_group_ind + 1;
cout << "New indices size: " << temp_indices.size() << endl;
super::set_input_cloud(temp_cloud, new_indices);
}
void eigen2cv( const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src, Mat& dst )
{
if( !(src.Flags & Eigen::RowMajorBit) )
{
Mat _src(src.cols(), src.rows(), DataType<_Tp>::type,
(void*)src.data(), src.stride()*sizeof(_Tp));
transpose(_src, dst);
}
else
{
Mat _src(src.rows(), src.cols(), DataType<_Tp>::type,
(void*)src.data(), src.stride()*sizeof(_Tp));
_src.copyTo(dst);
}
}
void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{
Mat cvmSumBuf;
cvgmSumBuf_.download(cvmSumBuf);
double* aHostTmp = (double*)cvmSumBuf.data;
//declare A and b
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
//retrieve A and b from cvmSumBuf
short sShift = 0;
for (int i = 0; i < 6; ++i){ // rows
for (int j = i; j < 7; ++j) { // cols + b
double value = aHostTmp[sShift++];
if (j == 6) // vector b
b.data()[i] = value;
else
A.data()[j * 6 + i] = A.data()[i * 6 + j] = value;
}//for each col
}//for each row
//checking nullspace
double dDet = A.determinant();
if (fabs(dDet) < 1e-15 || dDet != dDet){
if (dDet != dDet)
PRINTSTR("Failure -- dDet cannot be qnan. ");
//reset ();
return;
}//if dDet is rational
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX());
Eigen::Vector3f tinc = result.tail<3>();
//compose
//eivTwCur = Rinc * eivTwCur + tinc;
//eimrmRwCur = Rinc * eimrmRwCur;
Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_);
Eigen::Matrix3f eimRinv = peimRw_->transpose();
eivTinv = Rinc * eivTinv + tinc;
eimRinv = Rinc * eimRinv;
*peivTw_ = -eimRinv.transpose() * eivTinv;
*peimRw_ = eimRinv.transpose();
}
void test_sort_indices_desc(Eigen::Matrix<T,R,C> val) {
using stan::math::sort_indices_desc;
typedef Eigen::Matrix<AVAR,R,C> AVEC;
typedef Eigen::Matrix<double,R,C> VEC;
const size_t size = val.size();
AVEC x(size);
for(size_t i=0U; i<size; i++)
x.data()[i] = AVAR(val[i]);
std::vector<int> val_sorted = sort_indices_desc(val);
std::vector<int> x_sorted = sort_indices_desc(x);
for(size_t i=0U; i<size; i++)
EXPECT_EQ(val_sorted.data()[i],x_sorted.data()[i]);
for(size_t i=0U; i<size; i++)
for(size_t j=0U; j<size; j++)
if(val_sorted.data()[i] == val.data()[j])
EXPECT_EQ(x_sorted.data()[i],x.data()[j]);
else
EXPECT_FALSE(x_sorted.data()[i]==x.data()[j]);
}
void OptimizedSelfAdjointMatrix6x6f::rankUpdate(const Eigen::Matrix<float, 6, 1>& u, const float& alpha)
{
__m128 s = _mm_set1_ps(alpha);
__m128 v1234 = _mm_loadu_ps(u.data());
__m128 v56xx = _mm_loadu_ps(u.data() + 4);
__m128 v1212 = _mm_movelh_ps(v1234, v1234);
__m128 v3434 = _mm_movehl_ps(v1234, v1234);
__m128 v5656 = _mm_movelh_ps(v56xx, v56xx);
__m128 v1122 = _mm_mul_ps(s, _mm_unpacklo_ps(v1212, v1212));
_mm_store_ps(data + 0, _mm_add_ps(_mm_load_ps(data + 0), _mm_mul_ps(v1122, v1212)));
_mm_store_ps(data + 4, _mm_add_ps(_mm_load_ps(data + 4), _mm_mul_ps(v1122, v3434)));
_mm_store_ps(data + 8, _mm_add_ps(_mm_load_ps(data + 8), _mm_mul_ps(v1122, v5656)));
__m128 v3344 = _mm_mul_ps(s, _mm_unpacklo_ps(v3434, v3434));
_mm_store_ps(data + 12, _mm_add_ps(_mm_load_ps(data + 12), _mm_mul_ps(v3344, v3434)));
_mm_store_ps(data + 16, _mm_add_ps(_mm_load_ps(data + 16), _mm_mul_ps(v3344, v5656)));
__m128 v5566 = _mm_mul_ps(s, _mm_unpacklo_ps(v5656, v5656));
_mm_store_ps(data + 20, _mm_add_ps(_mm_load_ps(data + 20), _mm_mul_ps(v5566, v5656)));
}
bool operator() (const T* const q_coeffs, const T* const t_coeffs, const T* const s, T* residuals) const
{
T q_ceres[4] = {q_coeffs[3], q_coeffs[0], q_coeffs[1], q_coeffs[2]};
Eigen::Matrix<T,3,3> R;
ceres::QuaternionToRotation(q_ceres, ceres::ColumnMajorAdapter3x3(R.data()));
Eigen::Matrix<T,4,4> H = Eigen::Matrix<T,4,4>::Identity();
H.block(0,0,3,3) = *s * R;
H(0,3) = t_coeffs[0];
H(1,3) = t_coeffs[1];
H(2,3) = t_coeffs[2];
Eigen::Matrix<T,4,4> pred_H2 = H * m_H1.cast<T>() * H.inverse();
Eigen::Matrix<T,4,4> err_H = m_H2.cast<T>().inverse() * pred_H2;
Eigen::Matrix<T,3,3> err_R = err_H.block(0,0,3,3);
T roll, pitch, yaw;
mat2RPY(err_R, roll, pitch, yaw);
residuals[0] = err_H(0,3);
residuals[1] = err_H(1,3);
residuals[2] = err_H(2,3);
residuals[3] = roll;
residuals[4] = pitch;
residuals[5] = yaw;
return true;
}
std::shared_ptr<Geometry> generateTexQuadGeometry(
float width, float height, Eigen::Vector3f pos, Eigen::Matrix3f rot) {
Eigen::Matrix<float, 6, 3 + 2, Eigen::RowMajor> vertex;
GLfloat vertex_pos_uv[] = {
-1.0f, 0, -1.0f, 0, 1,
1.0f, 0, 1.0f, 1, 0,
-1.0f, 0, 1.0f, 0, 0,
1.0f, 0, 1.0f, 1, 0,
-1.0f, 0, -1.0f, 0, 1,
1.0f, 0, -1.0f, 1, 1,
};
for(int i = 0; i < 6; i++) {
Eigen::Vector3f p_local(
vertex_pos_uv[i * 5 + 0] * width / 2,
vertex_pos_uv[i * 5 + 1],
vertex_pos_uv[i * 5 + 2] * height / 2);
vertex.row(i).head(3) = rot * p_local + pos;
vertex.row(i).tail(2) = Eigen::Vector2f(
vertex_pos_uv[i * 5 + 3], vertex_pos_uv[i * 5 + 4]);
}
return Geometry::createPosUV(vertex.rows(), vertex.data());
}
void writeMatrix(Eigen::MatrixXd data, const std::string& filename,
double cell_size, BOX2D bounds, SpatialReference srs)
{
using namespace Eigen;
gdal::registerDrivers();
std::array<double, 6> pixelToPos;
pixelToPos[0] = bounds.minx;
pixelToPos[1] = cell_size;
pixelToPos[2] = 0.0;
pixelToPos[3] = bounds.miny;
pixelToPos[4] = 0.0;
pixelToPos[5] = cell_size;
gdal::Raster raster(filename, "GTiff", srs, pixelToPos);
gdal::GDALError err = raster.open(data.cols(), data.rows(), 1,
Dimension::Type::Float, -9999.0);
if (err != gdal::GDALError::None)
throw pdal_error(raster.errorMsg());
// Two things going on here. First, Eigen defaults to column major order,
// but GDALUtils expects row major, so we can convert it. Also, double
// doesn't seem to work for some reason, so maybe we go back and make the
// incoming matrix always be a float, but for now just cast it.
Eigen::Matrix<float, Dynamic, Dynamic, RowMajor> dataRowMajor;
dataRowMajor = data.cast<float>();
raster.writeBand((uint8_t *)dataRowMajor.data(), 1);
}
// The main mex-function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
// Characterize the type of the call
int callCharacter = -1;
const mxArray *data1 = prhs[0];
const mxArray *data2 = prhs[1];
const mwSize *data1dim = mxGetDimensions(data1);
const mwSize *data2dim = mxGetDimensions(data2);
//create three pointers to absolute, relative, and point_cloud adapters here
opengv::relative_pose::RelativeAdapterBase* relativeAdapter =
new opengv::relative_pose::MANoncentralRelative(
(double*) mxGetData(data1),
(double*) mxGetData(data2),
data1dim[1],
data2dim[1] );
nrelRansacPtr problem;
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter ) );
opengv::sac::Ransac<nrelRansac> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
ransac.max_iterations_ = 1000;
ransac.computeModel();
Eigen::Matrix<double,3,5> result;
result.block<3,4>(0,0) = ransac.model_coefficients_;
result(0,4) = ransac.iterations_;
int dims[2];
dims[0] = 3;
dims[1] = 5;
plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
memcpy(mxGetData(plhs[0]), result.data(), 15*sizeof(double));
}
// Matx case
template<typename _Tp, int _rows, int _cols, int _options, int _maxRows, int _maxCols> static inline
void cv2eigen( const Matx<_Tp, _rows, _cols>& src,
Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst )
{
if( !(dst.Flags & Eigen::RowMajorBit) )
{
const Mat _dst(_cols, _rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
}
else
{
const Mat _dst(_rows, _cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
Mat(src).copyTo(_dst);
}
}
void cv2eigen( const Matx<_Tp, _rows, _cols>& src,
Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst )
{
if( !(dst.Flags & Eigen::RowMajorBit) )
{
Mat _dst(_cols, _rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
else
{
Mat _dst(_rows, _cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
Mat(src).copyTo(_dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
}
//Matx
template<typename _Tp, int _cols> static inline
void cv2eigen( const Matx<_Tp, 1, _cols>& src,
Eigen::Matrix<_Tp, 1, Eigen::Dynamic>& dst )
{
dst.resize(_cols);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
const Mat _dst(_cols, 1, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
}
else
{
const Mat _dst(1, _cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
Mat(src).copyTo(_dst);
}
}
void cv2eigen( const Matx<_Tp, 1, _cols>& src,
Eigen::Matrix<_Tp, 1, Eigen::Dynamic>& dst )
{
dst.resize(_cols);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
Mat _dst(_cols, 1, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
else
{
Mat _dst(1, _cols, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
Mat(src).copyTo(_dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
}
std::string matrixString(const Eigen::Matrix<T, R, C>& m)
{
if (m.rows() > 0 && m.cols() > 0)
{
return std::string((char *) m.data(), m.size() * sizeof(T));
} else
{
return "";
}
}
void load( Archive & ar,
Eigen::Matrix<T,N,M> & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
int n1;
ar >> BOOST_SERIALIZATION_NVP( n1 );
ar >> make_array( t.data(), n0*n1 );
}
void save( Archive & ar,
const Eigen::Matrix<T,RowsAtCompileTime,ColsAtCompileTime,Options,MaxRowsAtCompileTime,MaxColsAtCompileTime> & t,
const unsigned int file_version )
{
int n0 = t.rows();
int n1 = t.cols();
ar << BOOST_SERIALIZATION_NVP( n0 );
ar << BOOST_SERIALIZATION_NVP( n1 );
ar << boost::serialization::make_array( t.data(), n0*n1 );
}
void grouped_vocabulary_tree<Point, K>::append_cloud(CloudPtrT& extra_cloud, vector<pair<int, int> >& indices, bool store_points)
{
// to begin with, we might have to remove nan points
Eigen::Matrix<float, super::rows, 1> p;
CloudPtrT temp_cloud(new CloudT);
temp_cloud->reserve(extra_cloud->size());
vector<pair<int, int> > temp_indices;
temp_indices.reserve(indices.size());
for (size_t i = 0; i < extra_cloud->size(); ++i) {
p = eig(extra_cloud->at(i));
if (std::find_if(p.data(), p.data()+super::rows, [] (float f) {
return std::isnan(f) || std::isinf(f);
}) == p.data()+super::rows) {
temp_cloud->push_back(extra_cloud->at(i));
temp_indices.push_back(indices[i]);
}
}
// assume these are all new groups
vector<int> new_indices(temp_indices.size());
pair<int, int> previous_p = make_pair(-1, -1);
int index_group_ind = nbr_groups - 1;
int counter = 0;
for (const pair<int, int>& p : temp_indices) {
// everything with this index pair should have the same label, assume ordered
if (p != previous_p) {
++index_group_ind;
index_group[index_group_ind] = p.first;
group_subgroup[index_group_ind] = p;
previous_p = p;
}
new_indices[counter] = index_group_ind;
++counter;
}
nbr_points += counter;
nbr_groups = index_group_ind + 1;
super::append_cloud(temp_cloud, new_indices, store_points);
}
inline void load(
Archive & ar,
Eigen::Matrix<S, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & g,
const unsigned int version)
{
int rows, cols;
ar & rows;
ar & cols;
g.resize(rows, cols);
ar & boost::serialization::make_array(g.data(), rows * cols);
}
void load( Archive & ar,
Eigen::Matrix<T,RowsAtCompileTime,ColsAtCompileTime,Options,MaxRowsAtCompileTime,MaxColsAtCompileTime> & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
int n1;
ar >> BOOST_SERIALIZATION_NVP( n1 );
t.resize( n0,n1 );
ar >> make_array( t.data(), n0*n1 );
}
