Matrix Matrix::estimate_quaternion(Matrix& A, Matrix& B, Matrix& A2, Matrix& B2) {
Matrix N1 = A.cross(B);
N1.normalize();
Matrix N2 = A2.cross(B2);
N2.normalize();
double cosa = N1.dot(N2.transposed()).get(0,0);
double sina = sqrt(0.5 - 0.5*cosa);
cosa = sqrt(0.5 + 0.5*cosa);
Matrix NN = N1.cross(N2);
NN.normalize();
NN *= sina;
double Q1_[] = {cosa, NN(0,0), NN(0,1), NN(0,2)};
Matrix Q1(1,4,Q1_);
A = A.quaternion_rotate(Q1);
B = B.quaternion_rotate(Q1);
cosa = A.dot(A2.transposed()).get(0,0);
sina = sqrt(0.5 - 0.5*cosa);
cosa = sqrt(0.5 + 0.5*cosa);
Matrix ax2 = A.cross(A2);
ax2.normalize();
ax2 *= sina;
double Q2_[] = {cosa, ax2(0,0), ax2(0,1), ax2(0,2)};
Matrix Q2(1, 4, Q2_);
Matrix Q = Q2.quaternion_multiply(Q1);
return Q;
}
void RigidAlignment::saveSphere(const char *dir)
{
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
Mesh *sphere = new Mesh();
sphere->openFile(m_spherename);
// matrix
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
sphere->rotation(ax3.fv(), deg);
sphere->rotation(axis2, m_rot[i * 3]);
// output
char filename[1024];
sprintf(filename, "%s/%s.vtk", dir, m_filename[i]);
sphere->saveFile(filename, "vtk");
delete sphere;
}
}
TopoDS_Shape CreateEllipsoid::executeCreation() const
{
try
{
gp_Pnt pnt(axis.Location());
gp_Dir dir(axis.Direction());
gp_Ax2 ax2(pnt, dir);
BRepPrimAPI_MakeSphere makeSphere(ax2, radius2, angle/180.0f*Standard_PI);
float scale = radius1 / radius2;
gp_Dir xDir = ax2.XDirection();
gp_Dir yDir = ax2.YDirection();
gp_GTrsf mat;
mat.SetValue(1, 1, xDir.X());
mat.SetValue(2, 1, xDir.Y());
mat.SetValue(3, 1, xDir.Z());
mat.SetValue(1, 2, yDir.X());
mat.SetValue(2, 2, yDir.Y());
mat.SetValue(3, 2, yDir.Z());
mat.SetValue(1, 3, dir.X()*scale);
mat.SetValue(2, 3, dir.Y()*scale);
mat.SetValue(3, 3, dir.Z()*scale);
BRepBuilderAPI_GTransform trs(makeSphere.Shape(), mat);
return trs.Shape();
}
catch(const StdFail_NotDone& ex)
{
throw Common::Exception(QObject::tr("Create ellipsoid error"));
}
}
TopoDS_Shape CreateSphere::executeCreation() const
{
try
{
gp_Ax2 ax2(axis.Location(), axis.Direction());
BRepPrimAPI_MakeSphere makeSphere(ax2, radius, angle/180.0f*Standard_PI);
return makeSphere.Shape();
}
catch(const StdFail_NotDone& ex)
{
throw Common::Exception(QObject::tr("Create sphere error"));
}
}
int OCCEdge::createEllipse(OCCStruct3d pnt, OCCStruct3d nor, double rMajor, double rMinor)
{
try {
gp_Ax2 ax2(gp_Pnt(pnt.x,pnt.y,pnt.z), gp_Dir(nor.x,nor.y,nor.z));
if (rMajor <= Precision::Confusion() || rMinor <= Precision::Confusion()) {
StdFail_NotDone::Raise("radius to small");
}
gce_MakeElips ellipse(ax2, rMajor, rMinor);
this->setShape(BRepBuilderAPI_MakeEdge(ellipse));
} catch(Standard_Failure &err) {
Handle_Standard_Failure e = Standard_Failure::Caught();
const Standard_CString msg = e->GetMessageString();
if (msg != NULL && strlen(msg) > 1) {
setErrorMessage(msg);
} else {
setErrorMessage("Failed to create ellipse");
}
return 0;
}
return 1;
}
osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry(
bool unique_state) {
typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
typename pcl::PointCloud<PointT>::ConstPtr xyz = this->getInputCloud<PointT>();
typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>();
if (xyz ==NULL) return NULL;
if (normals ==NULL) return NULL;
if (rads ==NULL) return NULL;
if (xyz->points.size() != normals->points.size()) return NULL;
if (rads->points.size() != normals->points.size()) return NULL;
pcl::IndicesConstPtr indices = indices_;
{
bool rebuild_indices= false;
if (indices_ == NULL) rebuild_indices=true;
else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
if (rebuild_indices){
pcl::IndicesPtr idxs(new std::vector<int>);
idxs->reserve(xyz->points.size());
for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
indices= idxs;
}
}
osg::Vec3Array* pts = new osg::Vec3Array;
osg::Vec3Array* npts = new osg::Vec3Array;
int fan_size = 1+ circle_cache.rows();
pts->reserve(indices->size()*fan_size );
npts->reserve(indices->size()*fan_size);
osg::Geometry* geom = new osg::Geometry;
for(int i=0, pstart=0; i<indices->size(); i++){
const int& idx = (*indices)[i];
const PointT& pt = xyz->points[idx];
const NormalT& npt = normals->points[idx];
pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
pcl::Normal nt;
Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
rot.row(2) << npt.getNormalVector3fMap().transpose();
Eigen::Vector3f ax2(1,0,0);
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,1,0;
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,0,1;
}
}
rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose();
rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized();
rot = rot*rads->points[idx].radius;
for(int j=0; j<circle_cache.rows(); j++){
Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap();
pts->push_back(osg::Vec3(apt[0],apt[1],apt[2]));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
}
geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart, fan_size ) );
pstart+= fan_size;
}
geom->setVertexArray( pts );
geom->setNormalArray(npts);
geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );
geom->setStateSet(stateset_);
return geom;
}
void RigidAlignment::update(void)
{
// new point
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
// new axis
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
// matrix
float mat[9];
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
{
Coordinate::rotation(ax3.fv(), deg, mat);
// axis rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
int id = m_point[i][j];
memcpy(p, m_sphere->vertex(id)->fv(), sizeof(float) * 3);
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// matrix
Coordinate::rotation(axis2, m_rot[i * 3], mat);
// rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// mean
memset(fmean, 0, sizeof(float) * m_nLM * 3);
for (int i = 0; i < m_nLM; i++)
for (int j = 0; j < m_nSubj; j++)
for (int k = 0; k < 3; k++)
fmean[i * 3 + k] += fpoint[(j * m_nLM + i) * 3 + k] / m_nSubj;
for (int i = 0; i < m_nLM; i++)
{
float norm = fmean[i * 3] * fmean[i * 3] + fmean[i * 3 + 1] * fmean[i * 3 + 1] + fmean[i * 3 + 2] * fmean[i * 3 + 2];
norm = sqrt(norm);
for (int j = 0; j < 3; j++)
fmean[i * 3 + j] /= norm;
}
/*for (int i = 0; i < m_nLM; i++)
cout << fmean[i * 3] << " " << fmean[i * 3 + 1] << " " << fmean[i * 3 + 2] << endl;*/
}
