本文整理汇总了C++中Point2d::lengthSquared方法的典型用法代码示例。如果您正苦于以下问题：C++ Point2d::lengthSquared方法的具体用法？C++ Point2d::lengthSquared怎么用？C++ Point2d::lengthSquared使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Point2d的用法示例。
在下文中一共展示了Point2d::lengthSquared方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: optimize
void MapPoint::optimize(MapResourcesManager * manager,
int numberIterations, const Frame & frame, const Point2f & positionOnFrame)
{
using namespace TMath;
TVectord oldPosition = m_position, v_f;
double chi2 = std::numeric_limits<double>::max();
TWLS<double> wls(3);
TVectord J_x(3), J_y(3);
Point2d e;
TMatrixd rotation(3, 3);
for (auto it = m_features.cbegin(); it != m_features.cend(); ++it)
manager->lock((*it)->keyFrame().get());
Point2d localPoint = frame.camera()->unproject(positionOnFrame);
for (int iteration = 0; iteration < numberIterations; ++iteration) {
wls.clear();
double new_chi2 = 0.0;
// compute residuals
{
rotation = frame.rotation();
v_f = rotation * m_position + frame.translation();
if (v_f(2) > std::numeric_limits<float>::epsilon()) {
const double z_inv_squared = 1.0 / (v_f(2) * v_f(2));
J_x(0) = (rotation(0, 0) * v_f(2) - rotation(2, 0) * v_f(0)) * z_inv_squared;
J_x(1) = (rotation(0, 1) * v_f(2) - rotation(2, 1) * v_f(0)) * z_inv_squared;
J_x(2) = (rotation(0, 2) * v_f(2) - rotation(2, 2) * v_f(0)) * z_inv_squared;
J_y(0) = (rotation(1, 0) * v_f(2) - rotation(2, 0) * v_f(1)) * z_inv_squared;
J_y(1) = (rotation(1, 1) * v_f(2) - rotation(2, 1) * v_f(1)) * z_inv_squared;
J_y(2) = (rotation(1, 2) * v_f(2) - rotation(2, 2) * v_f(1)) * z_inv_squared;
e = localPoint - LocationOptimizer::project2d(v_f);
wls.addMeasurement(e.x, J_x);
wls.addMeasurement(e.y, J_y);
new_chi2 += e.lengthSquared();
}
}
auto it = m_features.cbegin();
while (it != m_features.cend()) {
std::shared_ptr<const KeyFrame> keyFrame = (*it)->keyFrame();
TMath_assert(keyFrame && !keyFrame->isDeleted());
rotation = keyFrame->rotation();
v_f = rotation * m_position + keyFrame->translation();
if (v_f(2) < std::numeric_limits<float>::epsilon()) {
++it;
continue;
}
const double z_inv_squared = 1.0 / (v_f(2) * v_f(2));
J_x(0) = (rotation(0, 0) * v_f(2) - rotation(2, 0) * v_f(0)) * z_inv_squared;
J_x(1) = (rotation(0, 1) * v_f(2) - rotation(2, 1) * v_f(0)) * z_inv_squared;
J_x(2) = (rotation(0, 2) * v_f(2) - rotation(2, 2) * v_f(0)) * z_inv_squared;
J_y(0) = (rotation(1, 0) * v_f(2) - rotation(2, 0) * v_f(1)) * z_inv_squared;
J_y(1) = (rotation(1, 1) * v_f(2) - rotation(2, 1) * v_f(1)) * z_inv_squared;
J_y(2) = (rotation(1, 2) * v_f(2) - rotation(2, 2) * v_f(1)) * z_inv_squared;
e = LocationOptimizer::project2d((*it)->localDir()) - LocationOptimizer::project2d(v_f);
wls.addMeasurement(e.x, J_x);
wls.addMeasurement(e.y, J_y);
new_chi2 += e.lengthSquared();
++it;
}
wls.compute();
// check if error increased
if ((new_chi2 > chi2) || (std::isnan(wls.X()(0)))) {
m_position = oldPosition; // roll-back
break;
}
oldPosition = m_position;
m_position += wls.X();
chi2 = new_chi2;
}
for (auto it = m_features.cbegin(); it != m_features.cend(); ++it)
manager->unlock((*it)->keyFrame().get());
}