我有一个函数,可以使用本征3.3.7 从C ++中的3个向量计算转换矩阵。在此函数中,有两行导致程序另一部分中的“ 检测到堆栈崩溃”错误,而不是函数本身。奇怪的是,即使我不调用该函数,也会发生错误。如果我注释掉这两行,则不会发生错误。怎么会这样?
//Calculate transformation matrix from sensor space to robot
Eigen::Matrix4f GetSensor2Robot(Eigen::Vector3f & origin,
Eigen::Vector3f & xPoint, Eigen::Vector3f & yPoint){
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
Eigen::Vector3f offset = origin;
transformation.col(3).head<3>() = offset;
Eigen::Vector3f xAxis = xPoint - origin;
xAxis.normalize();
Eigen::Vector3f yAxis = yPoint - origin;
yAxis.normalize();
//Make axis orthogonal
float dot = xAxis.dot(yAxis);
// Force the dot product of the two input vectors to
// fall within the domain for inverse cosine, which
// is -1 <= x <= 1.
dot = ( dot < -1.0 ? -1.0 : ( dot > 1.0 ? 1.0 : dot ) );
//Differece to 90 degrees
double angleDiff = acos(dot) - M_PI/2;
Eigen::Vector3f zAxis = xAxis.cross(yAxis);
Eigen::AngleAxis<float> axisAngleX(angleDiff/2, zAxis);
//THE FOLLOWING LINE CAUSES THE ERROR
xAxis = axisAngleX.toRotationMatrix()*xAxis;
Eigen::AngleAxis<float> axisAngleY(-angleDiff/2, zAxis);
//THE FOLLOWING LINE CAUSES THE ERROR
yAxis = axisAngleY.toRotationMatrix()*yAxis;
transformation.col(0).head<3>() = xAxis;
transformation.col(1).head<3>() = yAxis;
transformation.col(2).head<3>() = zAxis;
Eigen::Matrix4f inverse = transformation.inverse();
return inverse;
}
实际发生错误的部分使用点云库:
pcl::MovingLeastSquares< pcl::PointXYZ, pcl::PointNormal > mls;
mls.setSearchMethod(organizedNeighbour);
mls.setComputeNormals(true);
mls.setInputCloud(inputCloud);
mls.setIndices(indices);
mls.setPolynomialOrder(1);
mls.setSearchRadius(0.1);
mls.setSqrGaussParam(0.1*0.1);
mls.setNumberOfThreads(MAX_NUMBER_OF_THREADS);
pcl::PointCloud<pcl::PointNormal>::Ptr smoothedCloud(new
pcl::PointCloud<pcl::PointNormal>());
//during the following call, the error occurs
mls.process(*smoothedCloud);
我在装有Eigen 3.3.4的另一台计算机上尝试了相同的代码,但未发生错误。