建設分野の最適化問題：3D点群データの平面フィッティングによる地形評価問題

import numpy as np
from naginterfaces.library import opt
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 座標データ
coordinates = np.array([
    [1.2, 2.5, 3.8], [2.3, 3.1, 4.1], [3.8, 1.9, 4.5],
    [3.2, 2.7, 4.0], [4.1, 2.3, 4.3], [3.7, 3.5, 4.4],
    [2.1, 3.8, 4.2], [3.5, 2.6, 4.6], [2.9, 3.0, 4.0],
    [3.0, 2.8, 4.5], [3.1, 3.4, 4.7], [1.8, 2.9, 4.1],
    [2.5, 3.2, 4.3], [3.4, 2.1, 4.2], [4.0, 3.3, 4.6]
])
n = len(coordinates)
epsilon = 1e-8  # 数値的安定性のための微小値

def objfun(mode, x, grad, _nstate):
    """ 目的関数：各点から平面までの距離の二乗和 """
    a, b, c, d = x
    norm = np.sqrt(a**2 + b**2 + c**2) + epsilon
    
    if mode in [0, 2]:
        plane_eq = np.dot(coordinates, [a, b, c]) + d
        distances = np.abs(plane_eq) / norm
        obj_val = np.sum(distances**2)
    else:
        obj_val = 0.0
    
    if mode == 2:
        sign = np.sign(plane_eq)
        common_term = 2 * sign * distances / norm**2
        grad_abc = np.sum(common_term[:, np.newaxis] * (coordinates * norm - [a, b, c] * plane_eq[:, np.newaxis] / norm), axis=0)
        grad_d = np.sum(2 * sign * distances / norm)
        grad[:] = np.concatenate([grad_abc, [grad_d]])
    
    return obj_val, grad

def confun(mode, needc, x, cjac, _nstate):
    """ 非線形制約：法線ベクトルの長さを1に制約 """
    a, b, c, _ = x
    
    if mode in [0, 2]:
        cons = np.array([a**2 + b**2 + c**2 - 1])
    else:
        cons = np.array([])
    
    if mode == 2:
        cjac[0, :] = [2*a, 2*b, 2*c, 0]
    
    return cons, cjac

# 初期値と境界条件の設定
x_init = [0, 0, 1, 0]
nvar, ncnln = 4, 1
lower_bounds = np.concatenate([-np.inf * np.ones(nvar), [0]])
upper_bounds = np.concatenate([np.inf * np.ones(nvar), [0]])

# ソルバーオプションの設定
comm = opt.nlp1_init('nlp1_solve')
opt.nlp1_option_string('Print Level = 0', comm)
opt.nlp1_option_string('Major Iteration Limit = 100', comm)

# ソルバーの呼び出し
result = opt.nlp1_solve(
    np.zeros((0, nvar)), lower_bounds, upper_bounds, objfun, x_init, comm,
    confun=confun, cjac=np.zeros((ncnln, nvar))
)

# 最適なパラメータと地形メトリクスの計算
a, b, c, d = result.x
normal = np.array([a, b, c])
normal /= np.linalg.norm(normal)
distances = np.abs(np.dot(coordinates, normal) + d)
slope_angle = np.arccos(abs(c)) * 180 / np.pi
slope_direction = np.array([a, b])
slope_direction /= np.linalg.norm(slope_direction)

# 目的関数の最適値を計算
obj_val, _ = objfun(0, result.x, None, None)

# 結果の出力
print("=== 建設現場地形評価 ===")
print(f"平面の方程式パラメータ: a={a:.6f}, b={b:.6f}, c={c:.6f}, d={d:.6f}")
print(f"目的関数の最適値: {obj_val:.6f}")
print(f"最大偏差: {np.max(distances):.6f} m")
print(f"平均偏差: {np.mean(distances):.6f} m")
print(f"標準偏差: {np.std(distances):.6f} m")
print(f"傾斜角度: {slope_angle:.6f}°")
print(f"傾斜の向き (X, Y): ({slope_direction[0]:.6f}, {slope_direction[1]:.6f})")
print("  注: 傾斜の向きは、XY平面上で最も急な下り勾配の方向を示します。")
print("      (1, 0)はX軸正方向、(0, 1)はY軸正方向を表します。")

# 3Dプロット
fig = plt.figure(figsize=(12, 9))
ax = fig.add_subplot(111, projection='3d')

scatter = ax.scatter(coordinates[:, 0], coordinates[:, 1], coordinates[:, 2], c=coordinates[:, 2], cmap='viridis', s=50)
plt.colorbar(scatter, label='Z value (m)')

x_range = np.linspace(coordinates[:, 0].min(), coordinates[:, 0].max(), 10)
y_range = np.linspace(coordinates[:, 1].min(), coordinates[:, 1].max(), 10)
X, Y = np.meshgrid(x_range, y_range)
Z = (-a * X - b * Y - d) / c
ax.plot_surface(X, Y, Z, alpha=0.5, cmap='coolwarm')

midpoint = coordinates[:, :2].mean(axis=0)
ax.quiver(midpoint[0], midpoint[1], Z.mean(), slope_direction[0], slope_direction[1], 0, 
          length=0.5, normalize=True, color='red', arrow_length_ratio=0.3)

ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Terrain and Optimal Plane')

plt.tight_layout()
plt.savefig('terrain_evaluation.png', dpi=300, bbox_inches='tight')
print("\n3Dプロットを 'terrain_evaluation.png' として保存しました。")
print("赤い矢印は傾斜の向きを示しています。")